test_gjk_libccd-inl_epa.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 
43 
44 #include <array>
45 #include <memory>
46 
47 #include <gtest/gtest.h>
48 
50 #include "expect_throws_message.h"
51 
52 namespace fcl {
53 namespace detail {
54 
55 class Polytope {
56  public:
58  polytope_ = new ccd_pt_t;
60  }
61 
63  // ccdPtDestroy() destroys the vertices, edges, and faces, contained in the
64  // polytope, allowing the polytope itself to be subsequently deleted.
66  delete polytope_;
67  }
68 
69  ccd_pt_vertex_t& v(int i) { return *v_[i]; }
70 
71  ccd_pt_edge_t& e(int i) { return *e_[i]; }
72 
73  ccd_pt_face_t& f(int i) { return *f_[i]; }
74 
75  ccd_pt_t& polytope() { return *polytope_; }
76 
77  const ccd_pt_vertex_t& v(int i) const { return *v_[i]; }
78 
79  const ccd_pt_edge_t& e(int i) const { return *e_[i]; }
80 
81  const ccd_pt_face_t& f(int i) const { return *f_[i]; }
82 
83  const ccd_pt_t& polytope() const { return *polytope_; }
84 
85  protected:
86  std::vector<ccd_pt_vertex_t*>& v() { return v_; }
87  std::vector<ccd_pt_edge_t*>& e() { return e_; }
88  std::vector<ccd_pt_face_t*>& f() { return f_; }
89 
90  private:
91  std::vector<ccd_pt_vertex_t*> v_;
92  std::vector<ccd_pt_edge_t*> e_;
93  std::vector<ccd_pt_face_t*> f_;
95 };
96 
103 class Tetrahedron : public Polytope {
104  public:
105  Tetrahedron(const std::array<fcl::Vector3<ccd_real_t>, 4>& vertices)
106  : Polytope() {
107  v().resize(4);
108  e().resize(6);
109  f().resize(4);
110  for (int i = 0; i < 4; ++i) {
111  v()[i] = ccdPtAddVertexCoords(&this->polytope(), vertices[i](0),
112  vertices[i](1), vertices[i](2));
113  }
114  e()[0] = ccdPtAddEdge(&polytope(), &v(0), &v(1));
115  e()[1] = ccdPtAddEdge(&polytope(), &v(1), &v(2));
116  e()[2] = ccdPtAddEdge(&polytope(), &v(2), &v(0));
117  e()[3] = ccdPtAddEdge(&polytope(), &v(0), &v(3));
118  e()[4] = ccdPtAddEdge(&polytope(), &v(1), &v(3));
119  e()[5] = ccdPtAddEdge(&polytope(), &v(2), &v(3));
120  f()[0] = ccdPtAddFace(&polytope(), &e(0), &e(1), &e(2));
121  f()[1] = ccdPtAddFace(&polytope(), &e(0), &e(3), &e(4));
122  f()[2] = ccdPtAddFace(&polytope(), &e(1), &e(4), &e(5));
123  f()[3] = ccdPtAddFace(&polytope(), &e(3), &e(5), &e(2));
124  }
125 };
126 
127 std::array<fcl::Vector3<ccd_real_t>, 4> EquilateralTetrahedronVertices(
128  ccd_real_t bottom_center_x, ccd_real_t bottom_center_y,
129  ccd_real_t bottom_center_z, ccd_real_t edge_length) {
130  std::array<fcl::Vector3<ccd_real_t>, 4> vertices;
131  auto compute_vertex = [bottom_center_x, bottom_center_y, bottom_center_z,
132  edge_length](ccd_real_t x, ccd_real_t y, ccd_real_t z,
133  fcl::Vector3<ccd_real_t>* vertex) {
134  *vertex << x * edge_length + bottom_center_x,
135  y * edge_length + bottom_center_y, z * edge_length + bottom_center_z;
136  };
137  compute_vertex(0.5, -0.5 / std::sqrt(3), 0, &vertices[0]);
138  compute_vertex(-0.5, -0.5 / std::sqrt(3), 0, &vertices[1]);
139  compute_vertex(0, 1 / std::sqrt(3), 0, &vertices[2]);
140  compute_vertex(0, 0, std::sqrt(2.0 / 3.0), &vertices[3]);
141  return vertices;
142 }
143 
144 // Produces a corrupted equilateral tetrahedron, but moves the top vertex to be
145 // on one of the bottom face's edges.
146 std::array<Vector3<ccd_real_t>, 4> TetrahedronColinearVertices() {
147  std::array<Vector3<ccd_real_t>, 4> vertices = EquilateralTetrahedronVertices(
148  0, 0, 0, 1);
149  vertices[3] = (vertices[0] + vertices[1]) / 2;
150  return vertices;
151 }
152 
153 // Produces a corrupted equilateral tetrahedron, but the bottom face is shrunk
154 // down too small to be trusted.
155 std::array<Vector3<ccd_real_t>, 4> TetrahedronSmallFaceVertices() {
156  std::array<Vector3<ccd_real_t>, 4> vertices = EquilateralTetrahedronVertices(
157  0, 0, 0, 1);
158  const ccd_real_t delta = constants<ccd_real_t>::eps() / 4;
159  for (int i = 0; i < 3; ++i) vertices[i] *= delta;
160  return vertices;
161 }
162 
184  public:
185  EquilateralTetrahedron(ccd_real_t bottom_center_x = 0,
186  ccd_real_t bottom_center_y = 0,
187  ccd_real_t bottom_center_z = 0,
188  ccd_real_t edge_length = 1)
190  bottom_center_x, bottom_center_y, bottom_center_z, edge_length)) {}
191 };
192 
194  for (int i = 0; i < 4; ++i) {
195  const ccd_vec3_t n =
197  for (int j = 0; j < 4; ++j) {
198  EXPECT_LE(ccdVec3Dot(&n, &p.v(j).v.v),
199  ccdVec3Dot(&n, &p.f(i).edge[0]->vertex[0]->v.v) +
201  }
202  }
203 }
204 
206  // Construct a equilateral tetrahedron, compute the normal on each face.
207  /*
208  p1-p4: The tetrahedron is positioned so that the origin is placed on each
209  face (some requiring flipping, some not)
210  p5: Origin is well within
211  p6: Origin on the bottom face, but the tetrahedron is too small; it must
212  evaluate all vertices and do a min/max comparison.
213  p7: Small tetrahedron with origin properly inside.
214  p8: Origin on the side face.
215  We do not test the case that the origin is on a vertex of the polytope. When
216  the origin coincides with a vertex, the two objects are touching, and we do
217  not need to call faceNormalPointOutward function to compute the direction
218  along which the polytope is expanded.
219  */
222  // Origin on the plane, and requires flipping the direction.
223  EquilateralTetrahedron p2(1.0 / 6, -1.0 / (6 * std::sqrt(3)),
224  -std::sqrt(6) / 9);
226  EquilateralTetrahedron p3(0, 1.0 / (3 * std::sqrt(3)), -std::sqrt(6) / 9);
228  EquilateralTetrahedron p4(-1.0 / 6, -1.0 / (6 * std::sqrt(3)),
229  -std::sqrt(6) / 9);
231 
232  // Check when the origin is within the polytope.
233  EquilateralTetrahedron p5(0, 0, -0.1);
235 
236  // Small tetrahedrons.
237  EquilateralTetrahedron p6(0, 0, 0, 0.01);
239  EquilateralTetrahedron p7(0, 0, -0.002, 0.01);
241  EquilateralTetrahedron p8(0, 0.01 / (3 * std::sqrt(3)),
242  -0.01 * std::sqrt(6) / 9, 0.01);
244 }
245 
246 GTEST_TEST(FCL_GJK_EPA, faceNormalPointingOutwardOriginNearFace1) {
247  // Creates a downward pointing tetrahedron which contains the origin. The
248  // origin is just below the "top" face of this tetrahedron. The remaining
249  // vertex is far enough away from the top face that it is considered a
250  // reliable witness to determine the direction of the face's normal. The top
251  // face is not quite parallel with the z = 0 plane. This test captures the
252  // failure condition reported in PR 334 -- a logic error made it so the
253  // reliable witness could be ignored.
254  const double face0_origin_distance = 0.005;
255  std::array<fcl::Vector3<ccd_real_t>, 4> vertices;
256  vertices[0] << 0.5, -0.5, face0_origin_distance;
257  vertices[1] << 0, 1, face0_origin_distance;
258  vertices[2] << -0.5, -0.5, face0_origin_distance;
259  vertices[3] << 0, 0, -1;
260  const double kPi = constants<double>::pi();
261  Eigen::AngleAxis<ccd_real_t> rotation(0.05 * kPi,
263  for (int i = 0; i < 4; ++i) {
264  vertices[i] = rotation * vertices[i];
265  }
266  Tetrahedron p(vertices);
267  {
268  // Make sure that the e₀ × e₁ points upward.
269  ccd_vec3_t f0_e0, f0_e1;
270  ccdVec3Sub2(&f0_e0, &(p.f(0).edge[0]->vertex[1]->v.v),
271  &(p.f(0).edge[0]->vertex[0]->v.v));
272  ccdVec3Sub2(&f0_e1, &(p.f(0).edge[1]->vertex[1]->v.v),
273  &(p.f(0).edge[1]->vertex[0]->v.v));
274  ccd_vec3_t f0_e0_cross_e1;
275  ccdVec3Cross(&f0_e0_cross_e1, &f0_e0, &f0_e1);
276  EXPECT_GE(f0_e0_cross_e1.v[2], 0);
277  }
279 }
280 
281 GTEST_TEST(FCL_GJK_EPA, faceNormalPointingOutwardOriginNearFace2) {
282  // Similar to faceNormalPointingOutwardOriginNearFace1 with an important
283  // difference: the fourth vertex is no longer a reliable witness; it lies
284  // within the distance tolerance. However, it is unambiguously farther off the
285  // plane of the top face than those that form the face. This confirms that
286  // when there are no obviously reliable witness that the most distant point
287  // serves.
288  const double face0_origin_distance = 0.005;
289  std::array<fcl::Vector3<ccd_real_t>, 4> vertices;
290  vertices[0] << 0.5, -0.5, face0_origin_distance;
291  vertices[1] << 0, 1, face0_origin_distance;
292  vertices[2] << -0.5, -0.5, face0_origin_distance;
293  vertices[3] << 0, 0, -0.001;
294 
295  Tetrahedron p(vertices);
297 }
298 
299 // Tests the error condition for this operation -- i.e., a degenerate triangle
300 // in a polytope.
301 GTEST_TEST(FCL_GJK_EPA, faceNormalPointingOutwardError) {
302  {
304 
305  // Degenerate triangle (in this case, co-linear vertices) in polytope.
306  // By construction, face 1 is the triangle that has been made degenerate.
309  &bad_tet.f(1)),
311  ".*faceNormalPointingOutward.*zero-area.*");
312  }
313 
314  {
316 
317  // Degenerate triangle (in this case, a face is too small) in polytope.
318  // By construction, face 1 is the triangle that has been made degenerate.
321  &bad_tet.f(1)),
323  ".*faceNormalPointingOutward.*zero-area.*");
324  }
325 }
326 
328  auto CheckSupportEPADirection = [](
329  const ccd_pt_t* polytope, const ccd_pt_el_t* nearest_pt,
330  const Eigen::Ref<const Vector3<ccd_real_t>>& dir_expected,
331  ccd_real_t tol) {
332  const ccd_vec3_t dir =
333  libccd_extension::supportEPADirection(polytope, nearest_pt);
334  for (int i = 0; i < 3; ++i) {
335  EXPECT_NEAR(dir.v[i], dir_expected(i), tol);
336  }
337  };
338 
339  // Nearest point is on the bottom triangle.
340  // The sampled direction should be -z unit vector.
341  EquilateralTetrahedron p1(0, 0, -0.1);
342  // The computation on Mac is very imprecise, thus the tolerance is big.
343  // TODO(hongkai.dai@tri.global): this tolerance should be cranked up once
344  // #291 is resolved.
345  const ccd_real_t tol = 3E-5;
346  CheckSupportEPADirection(&p1.polytope(),
347  reinterpret_cast<const ccd_pt_el_t*>(&p1.f(0)),
348  Vector3<ccd_real_t>(0, 0, -1), tol);
349  // Nearest point is on an edge, as the origin is on an edge.
350  EquilateralTetrahedron p2(0, 0.5 / std::sqrt(3), 0);
351  // e(0) has two neighbouring faces, f(0) and f(1). The support direction could
352  // be the normal direction of either face.
353  if (p2.e(0).faces[0] == &p2.f(0)) {
354  // Check the support direction, should be the normal direction of f(0).
355  CheckSupportEPADirection(&p2.polytope(),
356  reinterpret_cast<const ccd_pt_el_t*>(&p2.e(0)),
357  Vector3<ccd_real_t>(0, 0, -1), tol);
358  } else {
359  // The support direction should be the normal direction of f(1)
360  CheckSupportEPADirection(
361  &p2.polytope(), reinterpret_cast<const ccd_pt_el_t*>(&p2.e(0)),
362  Vector3<ccd_real_t>(0, -2 * std::sqrt(2) / 3, 1.0 / 3), tol);
363  }
364  // Nearest point is on a vertex, should throw an error.
365  EquilateralTetrahedron p3(-0.5, 0.5 / std::sqrt(3), 0);
366  EXPECT_THROW(
368  &p3.polytope(), reinterpret_cast<const ccd_pt_el_t*>(&p3.v(0))),
370 
371  // Origin is an internal point of the bottom triangle
372  EquilateralTetrahedron p4(0, 0, 0);
373  CheckSupportEPADirection(&p4.polytope(),
374  reinterpret_cast<const ccd_pt_el_t*>(&p4.f(0)),
375  Vector3<ccd_real_t>(0, 0, -1), tol);
376 
377  // Nearest point is on face(1)
378  EquilateralTetrahedron p5(0, 1 / (3 * std::sqrt(3)),
379  -std::sqrt(6) / 9 + 0.01);
380  CheckSupportEPADirection(
381  &p5.polytope(), reinterpret_cast<const ccd_pt_el_t*>(&p5.f(1)),
382  Vector3<ccd_real_t>(0, -2 * std::sqrt(2) / 3, 1.0 / 3), tol);
383 }
384 
385 GTEST_TEST(FCL_GJK_EPA, supportEPADirectionError) {
387  // Note: the exception is only thrown if the nearest feature's disatance is
388  // zero.
389  tet.v(1).dist = 0;
392  &tet.polytope(), reinterpret_cast<ccd_pt_el_t*>(&tet.v(1))),
394  ".+supportEPADirection.+nearest point to the origin is a vertex.+");
395 }
396 
399 
400  auto CheckPointOutsidePolytopeFace = [&p](ccd_real_t x, ccd_real_t y,
401  ccd_real_t z, int face_index,
402  bool is_outside_expected) {
403  ccd_vec3_t pt;
404  pt.v[0] = x;
405  pt.v[1] = y;
406  pt.v[2] = z;
408  &p.f(face_index), &pt),
409  is_outside_expected);
410  };
411 
412  const bool expect_inside = false;
413  const bool expect_outside = true;
414  // point (0, 0, 0.1) is inside the tetrahedron.
415  CheckPointOutsidePolytopeFace(0, 0, 0.1, 0, expect_inside);
416  CheckPointOutsidePolytopeFace(0, 0, 0.1, 1, expect_inside);
417  CheckPointOutsidePolytopeFace(0, 0, 0.1, 2, expect_inside);
418  CheckPointOutsidePolytopeFace(0, 0, 0.1, 3, expect_inside);
419 
420  // point(0, 0, 2) is outside the tetrahedron. But it is on the "inner" side
421  // of the bottom face.
422  CheckPointOutsidePolytopeFace(0, 0, 2, 0, expect_inside);
423  CheckPointOutsidePolytopeFace(0, 0, 2, 1, expect_outside);
424  CheckPointOutsidePolytopeFace(0, 0, 2, 2, expect_outside);
425  CheckPointOutsidePolytopeFace(0, 0, 2, 3, expect_outside);
426 
427  // point (0, 0, 0) is right on the bottom face.
428  CheckPointOutsidePolytopeFace(0, 0, 0, 0, expect_inside);
429  CheckPointOutsidePolytopeFace(0, 0, 0, 1, expect_inside);
430  CheckPointOutsidePolytopeFace(0, 0, 0, 2, expect_inside);
431  CheckPointOutsidePolytopeFace(0, 0, 0, 3, expect_inside);
432 }
433 
434 // Tests against a degenerate polytope.
435 GTEST_TEST(FCL_GJK_EPA, isOutsidePolytopeFaceError) {
436  // The test point doesn't matter; it'll never get that far.
437  // NOTE: For platform compatibility, the assertion message is pared down to
438  // the simplest component: the actual function call in the assertion.
439  ccd_vec3_t pt{{10, 10, 10}};
440 
441  {
443 
444  // Degenerate triangle (in this case, co-linear vertices) in polytope.
445  // By construction, face 1 is the triangle that has been made degenerate.
448  &bad_tet.f(1), &pt),
450  ".*faceNormalPointingOutward.*zero-area.*");
451  }
452 
453  {
455 
456  // Degenerate triangle (in this case, a face is too small) in polytope.
457  // By construction, face 1 is the triangle that has been made degenerate.
460  &bad_tet.f(0), &pt),
462  ".*faceNormalPointingOutward.*zero-area.*");
463  }
464 }
465 
466 // Construct a polytope with the following shape, namely an equilateral triangle
467 // on the top, and an equilateral triangle of the same size, but rotate by 60
468 // degrees on the bottom. We will then connect the vertices of the equilateral
469 // triangles to form a convex polytope.
470 // v₄
471 // v₃__╱╲__v₅
472 // ╲╱ ╲╱
473 // ╱____╲
474 // v₂ ╲╱ v₀
475 // v₁
476 // The edges are in this order connected in a counter-clockwise order.
477 // e(0) connects v(0) and v(2).
478 // e(1) connects v(2) and v(4).
479 // e(2) connects v(4) and v(0).
480 // e(3) connects v(1) and v(3).
481 // e(4) connects v(3) and v(5).
482 // e(5) connects v(5) and v(1).
483 // e(6 + i) connects v(i) and v(i + 1).
484 // f(0) is the upper triangle.
485 // f(1) is the lower triangle.
486 // f(2 + i) is the triangle that connects v(i), v(i + 1) and v(i + 2), namely
487 // the triangles on the side.
488 // For each face, the edge e(0).cross(e(1)) has the following direction
489 // f(0) points inward.
490 // f(1) points outward.
491 // f(2) points inward.
492 // f(3) points outward.
493 // f(4) points inward.
494 // f(5) points outward.
495 // f(6) points inward
496 // f(7) points outward.
497 class Hexagram : public Polytope {
498  public:
499  Hexagram(ccd_real_t bottom_center_x = 0, ccd_real_t bottom_center_y = 0,
500  ccd_real_t bottom_center_z = 0)
501  : Polytope() {
502  v().resize(6);
503  e().resize(12);
504  f().resize(8);
505  auto AddHexagramVertex = [bottom_center_x, bottom_center_y, bottom_center_z,
506  this](ccd_real_t x, ccd_real_t y, ccd_real_t z) {
507  return ccdPtAddVertexCoords(&this->polytope(), x + bottom_center_x,
508  y + bottom_center_y, z + bottom_center_z);
509  };
510  // right corner of upper triangle
511  v()[0] = AddHexagramVertex(0.5, -1 / std::sqrt(3), 1);
512  // bottom corner of lower triangle
513  v()[1] = AddHexagramVertex(0, -2 / std::sqrt(3), 0);
514  // left corner of upper triangle
515  v()[2] = AddHexagramVertex(-0.5, -1 / std::sqrt(3), 1);
516  // left corner of lower triangle
517  v()[3] = AddHexagramVertex(-0.5, 1 / std::sqrt(3), 0);
518  // top corner of upper triangle
519  v()[4] = AddHexagramVertex(0, 2 / std::sqrt(3), 1);
520  // right corner of lower triangle
521  v()[5] = AddHexagramVertex(0.5, 1 / std::sqrt(3), 0);
522 
523  // edges on the upper triangle
524  e()[0] = ccdPtAddEdge(&polytope(), &v(0), &v(2));
525  e()[1] = ccdPtAddEdge(&polytope(), &v(2), &v(4));
526  e()[2] = ccdPtAddEdge(&polytope(), &v(4), &v(0));
527  // edges on the lower triangle
528  e()[3] = ccdPtAddEdge(&polytope(), &v(1), &v(3));
529  e()[4] = ccdPtAddEdge(&polytope(), &v(3), &v(5));
530  e()[5] = ccdPtAddEdge(&polytope(), &v(5), &v(1));
531  // edges connecting the upper triangle to the lower triangle
532  for (int i = 0; i < 6; ++i) {
533  e()[6 + i] = ccdPtAddEdge(&polytope(), &v(i), &v((i + 1) % 6));
534  }
535 
536  // upper triangle
537  f()[0] = ccdPtAddFace(&polytope(), &e(0), &e(1), &e(2));
538  // lower triangle
539  f()[1] = ccdPtAddFace(&polytope(), &e(3), &e(4), &e(5));
540  // triangles on the side
541  f()[2] = ccdPtAddFace(&polytope(), &e(0), &e(7), &e(6));
542  f()[3] = ccdPtAddFace(&polytope(), &e(7), &e(8), &e(3));
543  f()[4] = ccdPtAddFace(&polytope(), &e(8), &e(9), &e(1));
544  f()[5] = ccdPtAddFace(&polytope(), &e(9), &e(10), &e(4));
545  f()[6] = ccdPtAddFace(&polytope(), &e(10), &e(11), &e(2));
546  f()[7] = ccdPtAddFace(&polytope(), &e(11), &e(6), &e(5));
547  }
548 };
549 
550 template <typename T>
551 bool IsElementInSet(const std::unordered_set<T*>& S, const T* element) {
552  return S.count(const_cast<T*>(element)) > 0;
553 }
554 
555 // @param border_edge_indices_expected
556 // polytope.e(border_edge_indices_expected(i)) is a border edge. Similarly for
557 // visible_face_indices_expected and internal_edges_indices_expected.
559  const Polytope& polytope,
560  const std::unordered_set<ccd_pt_edge_t*>& border_edges,
561  const std::unordered_set<ccd_pt_face_t*>& visible_faces,
562  const std::unordered_set<ccd_pt_edge_t*>& internal_edges,
563  const std::unordered_set<int>& border_edge_indices_expected,
564  const std::unordered_set<int>& visible_face_indices_expected,
565  const std::unordered_set<int> internal_edges_indices_expected) {
566  // Check border_edges
567  EXPECT_EQ(border_edges.size(), border_edge_indices_expected.size());
568  for (const int edge_index : border_edge_indices_expected) {
569  EXPECT_TRUE(IsElementInSet(border_edges, &polytope.e(edge_index)));
570  }
571  // Check visible_faces
572  EXPECT_EQ(visible_faces.size(), visible_face_indices_expected.size());
573  for (const int face_index : visible_face_indices_expected) {
574  EXPECT_TRUE(IsElementInSet(visible_faces, &polytope.f(face_index)));
575  }
576  // Check internal_edges
577  EXPECT_EQ(internal_edges.size(), internal_edges_indices_expected.size());
578  for (const auto edge_index : internal_edges_indices_expected) {
579  EXPECT_TRUE(IsElementInSet(internal_edges, &polytope.e(edge_index)));
580  }
581 }
582 
583 // @param edge_indices we will call ComputeVisiblePatchRecursive(polytope, face,
584 // edge_index, new_vertex, ...) for each edge_index in edge_indices. Namely we
585 // will compute the visible patches, starting from face.e(edge_index).
587  const Polytope& polytope, ccd_pt_face_t& face,
588  const std::vector<int>& edge_indices, const ccd_vec3_t& new_vertex,
589  const std::unordered_set<int>& border_edge_indices_expected,
590  const std::unordered_set<int>& visible_face_indices_expected,
591  const std::unordered_set<int>& internal_edges_indices_expected) {
592  std::unordered_set<ccd_pt_edge_t*> border_edges;
593  std::unordered_set<ccd_pt_face_t*> visible_faces;
594  std::unordered_set<ccd_pt_face_t*> hidden_faces;
595  visible_faces.insert(&face);
596  std::unordered_set<ccd_pt_edge_t*> internal_edges;
597  for (const int edge_index : edge_indices) {
599  polytope.polytope(), face, edge_index, new_vertex, &border_edges,
600  &visible_faces, &hidden_faces, &internal_edges);
601  }
602  CheckComputeVisiblePatchCommon(polytope, border_edges, visible_faces,
603  internal_edges, border_edge_indices_expected,
604  visible_face_indices_expected,
605  internal_edges_indices_expected);
606 
607  // Confirm that visible and hidden faces are disjoint sets.
608  for (const auto hidden_face : hidden_faces) {
609  EXPECT_EQ(visible_faces.count(hidden_face), 0u);
610  }
611 }
612 
614  const Polytope& polytope, ccd_pt_face_t& face, const ccd_vec3_t& new_vertex,
615  const std::unordered_set<int>& border_edge_indices_expected,
616  const std::unordered_set<int>& visible_face_indices_expected,
617  const std::unordered_set<int>& internal_edges_indices_expected) {
618  std::unordered_set<ccd_pt_edge_t*> border_edges;
619  std::unordered_set<ccd_pt_face_t*> visible_faces;
620  std::unordered_set<ccd_pt_edge_t*> internal_edges;
621  libccd_extension::ComputeVisiblePatch(polytope.polytope(), face, new_vertex,
622  &border_edges, &visible_faces,
623  &internal_edges);
624 
625  CheckComputeVisiblePatchCommon(polytope, border_edges, visible_faces,
626  internal_edges, border_edge_indices_expected,
627  visible_face_indices_expected,
628  internal_edges_indices_expected);
629 }
630 
631 GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatch_TopFaceVisible) {
632  // 1 visible face.
633  Hexagram hex;
634  // Point P is just slightly above the top triangle. Only the top triangle can
635  // be seen from point P.
636  ccd_vec3_t p;
637  p.v[0] = 0;
638  p.v[1] = 0;
639  p.v[2] = 1.1;
640  const std::unordered_set<int> empty_set;
641  // Test recursive implementation.
642  CheckComputeVisiblePatchRecursive(hex, hex.f(0), {0}, p, {0}, {0}, empty_set);
643 
644  // Test ComputeVisiblePatch.
645  CheckComputeVisiblePatch(hex, hex.f(0), p, {0, 1, 2}, {0}, empty_set);
646 }
647 
648 GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatch_4FacesVisible) {
649  // 4 visible faces.
650  Hexagram hex;
651  // Point P is just above the top triangle by a certain height, such that it
652  // can see the triangles on the side, which connects two vertices on the upper
653  // triangle, and one vertex on the lower triangle.
654  ccd_vec3_t p;
655  p.v[0] = 0;
656  p.v[1] = 0;
657  p.v[2] = 2.1;
658  // Test recursive implementation.
659  CheckComputeVisiblePatchRecursive(hex, hex.f(0), {0}, p, {6, 7}, {0, 2}, {0});
660 
661  // Test ComputeVisiblePatch.
662  CheckComputeVisiblePatch(hex, hex.f(0), p, {6, 7, 8, 9, 10, 11}, {0, 2, 4, 6},
663  {0, 1, 2});
664 }
665 
666 GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatch_TopAndSideFacesVisible) {
667  // 2 visible faces.
668  Hexagram hex;
669  // Point P is just outside the upper triangle (face0) and the triangle face2,
670  // it can see both face0 and face2, but not the other triangles.
671  ccd_vec3_t p;
672  p.v[0] = 0;
673  p.v[1] = -1 / std::sqrt(3) - 0.1;
674  p.v[2] = 1.1;
675  CheckComputeVisiblePatchRecursive(hex, hex.f(0), {0}, p, {6, 7}, {0, 2}, {0});
676 
677  CheckComputeVisiblePatch(hex, hex.f(0), p, {1, 2, 6, 7}, {0, 2}, {0});
678 }
679 
680 GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatch_2FacesVisible) {
681  // Test with the equilateral tetrahedron.
682  // Point P is outside of an edge on the bottom triangle. It can see both faces
683  // neighbouring that edge.
684 
685  EquilateralTetrahedron tetrahedron(0, 0, -0.1);
686  ccd_vec3_t p;
687  p.v[0] = 0;
688  p.v[1] = -1 / std::sqrt(3) - 0.1;
689  p.v[2] = -0.2;
690 
691  // Start with from face 0.
692  CheckComputeVisiblePatch(tetrahedron, tetrahedron.f(0), p, {1, 2, 3, 4},
693  {0, 1}, {0});
694 
695  // Start with from face 1.
696  CheckComputeVisiblePatch(tetrahedron, tetrahedron.f(1), p, {1, 2, 3, 4},
697  {0, 1}, {0});
698 }
699 
700 /*
701  * Given an equilateral tetrahedron, create a query point that is co-linear with
702  * edge 0 as q = v₀ + ρ(v₀ - v₁), confirms that the correct tetrahedra faces are
703  * included in the visible patch. Point q is co-linear with edge 0 which is
704  * adjacent to faces f0 and f1. Face f3 is trivially visible from q.
705  *
706  * If the query point is co-linear with a tet edge, then both adjacent faces
707  * should be visible. The behavior is sensitive to numerical accuracy issues and
708  * we expose rho (ρ) as a parameter so that different scenarios can easily be
709  * authored which exercise different code paths to determine visibility. (In the
710  * code, "visibility" may be determined by multiple tests.)
711  */
713  double rho) {
714  // A new vertex is colinear with an edge e[0] of the tetrahedron. The border
715  // edges should be e[1], e[4], e[5]. The visible faces should be f[0], f[1],
716  // f[3], and the internal edges should be e[0], e[2], e[3].
717  // For the numbering of the edges/vertices/faces in the equilateral
718  // tetrahedron, please refer to the documentation of EquilateralTetrahedron.
719  ccd_vec3_t query_point;
720  for (int i = 0; i < 3; ++i) {
721  query_point.v[i] = (1 + rho) * tet.v(0).v.v.v[i] - rho * tet.v(1).v.v.v[i];
722  }
723  std::unordered_set<ccd_pt_edge_t*> border_edges;
724  std::unordered_set<ccd_pt_face_t*> visible_faces;
725  std::unordered_set<ccd_pt_edge_t*> internal_edges;
726  libccd_extension::ComputeVisiblePatch(tet.polytope(), tet.f(3), query_point,
727  &border_edges, &visible_faces,
728  &internal_edges);
729 
730  EXPECT_EQ(border_edges.size(), 3u);
731  EXPECT_EQ(border_edges, std::unordered_set<ccd_pt_edge_t*>(
732  {&(tet.e(1)), &(tet.e(4)), &(tet.e(5))}));
733  EXPECT_EQ(visible_faces.size(), 3u);
734  EXPECT_EQ(visible_faces, std::unordered_set<ccd_pt_face_t*>(
735  {&(tet.f(0)), &(tet.f(1)), &(tet.f(3))}));
736  EXPECT_EQ(internal_edges.size(), 3u);
737  EXPECT_EQ(internal_edges, std::unordered_set<ccd_pt_edge_t*>(
738  {&(tet.e(0)), &(tet.e(2)), &(tet.e(3))}));
739 }
740 
741 GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatchColinearNewVertex) {
742  // Case 1: Visibility of faces f0 and f1 is not immediately apparent --
743  // requires recognition that q, v0, and v1 are colinear.
744  EquilateralTetrahedron tet1(-0.05, -0.13, 0.12);
746  // Case 2: Visibility of faces f0 and f1 are independently confirmed --
747  // colinearity doesn't matter.
748  EquilateralTetrahedron tet2(0.1, 0.2, 0.3);
750 }
751 
752 // Tests that the sanity check causes `ComputeVisiblePatch()` to throw in
753 // debug builds.
754 GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatchSanityCheck) {
755 #ifndef NDEBUG
756  // NOTE: The sanity check function only gets compiled in debug mode.
758  std::unordered_set<ccd_pt_edge_t*> border_edges;
759  std::unordered_set<ccd_pt_face_t*> visible_faces;
760  std::unordered_set<ccd_pt_edge_t*> internal_edges;
761 
762  // Top view labels of vertices, edges and faces.
763  // .
764  // v2 .
765  // /\ /\ .
766  // / |\ / |\ .
767  // /e5| \ / | \ .
768  // e2 / | \ / |f2\ .
769  // / ╱╲v3 \e1 /f3 ╱╲ \ // f0 is the bottom face. .
770  // / ╱ ╲ \ / ╱ ╲ \ .
771  // / ╱e3 e4╲ \ / ╱ f1 ╲ \ .
772  // /╱____________╲\ /╱____________╲\ .
773  // v0 e0 v1 .
774  //
775  // The tet is centered on the origin, pointing upwards (seen from above in
776  // the diagram above. We define a point above this to define the visible patch
777  // The *correct* patch should have the following:
778  // visible faces: f1, f2, f3
779  // internal edges: e3, e4, e5
780  // border edges: e0, e1, e2
781  auto set_ideal = [&border_edges, &internal_edges, &visible_faces, &tet]() {
782  border_edges =
783  std::unordered_set<ccd_pt_edge_t*>{&tet.e(0), &tet.e(1), &tet.e(2)};
784  internal_edges =
785  std::unordered_set<ccd_pt_edge_t*>{&tet.e(3), &tet.e(4), &tet.e(5)};
786  visible_faces =
787  std::unordered_set<ccd_pt_face_t*>{&tet.f(1), &tet.f(2), &tet.f(3)};
788  };
789 
790  set_ideal();
792  tet.polytope(), border_edges, visible_faces, internal_edges));
793 
794  // Failure conditions:
795  // Two adjacent faces have edge e --> e is internal edge.
796  set_ideal();
797  internal_edges.erase(&tet.e(5));
799  tet.polytope(), border_edges, visible_faces, internal_edges));
800 
801  // Edge in internal edge --> two adjacent faces visible.
802  set_ideal();
803  visible_faces.erase(&tet.f(3));
805  tet.polytope(), border_edges, visible_faces, internal_edges));
806 
807  // Edge in border_edges --> one (and only one) visible face.
808  set_ideal();
809  internal_edges.erase(&tet.e(5));
810  border_edges.insert(&tet.e(5));
812  tet.polytope(), border_edges, visible_faces, internal_edges));
813 
814 #endif // NDEBUG
815 }
816 
817 // Returns true if the the distance difference between the two vertices are
818 // below tol.
820  const ccd_pt_vertex_t* v2, ccd_real_t tol) {
821  return ccdVec3Dist2(&v1->v.v, &v2->v.v) < tol * tol;
822 }
823 
824 // Return true, if the vertices in e1 are all mapped to the vertices in e2,
825 // according to the mapping @p map_v1_to_v2.
826 bool EdgeMatch(const ccd_pt_edge_t* e1, const ccd_pt_edge_t* e2,
827  const std::unordered_map<ccd_pt_vertex_t*, ccd_pt_vertex_t*>&
828  map_v1_to_v2) {
829  ccd_pt_vertex_t* v2_expected[2];
830  for (int i = 0; i < 2; ++i) {
831  auto it = map_v1_to_v2.find(e1->vertex[i]);
832  if (it == map_v1_to_v2.end()) {
833  throw std::logic_error("vertex[" + std::to_string(i) +
834  "] in e1 is not found in map_v1_to_v2");
835  }
836  v2_expected[i] = it->second;
837  }
838  return (v2_expected[0] == e2->vertex[0] && v2_expected[1] == e2->vertex[1]) ||
839  (v2_expected[0] == e2->vertex[1] && v2_expected[1] == e2->vertex[0]);
840 }
841 
842 // Return true, if the edges in f1 are all mapped to the edges in f2, according
843 // to the mapping @p map_e1_to_e2.
845  const ccd_pt_face_t* f1, const ccd_pt_face_t* f2,
846  const std::unordered_map<ccd_pt_edge_t*, ccd_pt_edge_t*>& map_e1_to_e2) {
847  std::unordered_set<ccd_pt_edge_t*> e2_expected;
848  for (int i = 0; i < 3; ++i) {
849  auto it = map_e1_to_e2.find(f1->edge[i]);
850  if (it == map_e1_to_e2.end()) {
851  throw std::logic_error("edge[" + std::to_string(i) +
852  "] in f1 is not found in map_e1_to_e2");
853  }
854  e2_expected.insert(it->second);
855  }
856  // The edges in f1 have to be distinct.
857  EXPECT_EQ(e2_expected.size(), 3u);
858  for (int i = 0; i < 3; ++i) {
859  auto it = e2_expected.find(f2->edge[i]);
860  if (it == e2_expected.end()) {
861  return false;
862  }
863  }
864  return true;
865 }
866 
867 // Construct the mapping from feature1_list to feature2_list. There should be a
868 // one-to-one correspondence between feature1_list and feature2_list.
869 // @param feature1_list[in] A list of features to be mapped from.
870 // @param feature2_list[in] A list of features to be mapped to.
871 // @param cmp_feature[in] Returns true if two features are identical, otherwise
872 // returns false.
873 // @param feature1[out] The set of features in feature1_list.
874 // @param feature2[out] The set of features in feature2_list.
875 // @param map_feature1_to_feature2[out] Maps a feature in feature1_list to
876 // a feature in feature2_list.
877 // @note The features in feature1_list should be unique, so are in
878 // feature2_list.
879 template <typename T>
881  const ccd_list_t* feature1_list, const ccd_list_t* feature2_list,
882  std::function<bool(const T*, const T*)> cmp_feature,
883  std::unordered_set<T*>* feature1, std::unordered_set<T*>* feature2,
884  std::unordered_map<T*, T*>* map_feature1_to_feature2) {
885  feature1->clear();
886  feature2->clear();
887  map_feature1_to_feature2->clear();
888  T* f;
889  ccdListForEachEntry(feature1_list, f, T, list) {
890  auto it = feature1->find(f);
891  assert(it == feature1->end());
892  feature1->emplace_hint(it, f);
893  }
894  ccdListForEachEntry(feature2_list, f, T, list) {
895  auto it = feature2->find(f);
896  assert(it == feature2->end());
897  feature2->emplace_hint(it, f);
898  }
899  EXPECT_EQ(feature1->size(), feature2->size());
900  for (const auto& f1 : *feature1) {
901  bool found_match = false;
902  for (const auto& f2 : *feature2) {
903  if (cmp_feature(f1, f2)) {
904  if (!found_match) {
905  map_feature1_to_feature2->emplace_hint(
906  map_feature1_to_feature2->end(), f1, f2);
907  found_match = true;
908  } else {
909  GTEST_FAIL() << "There should be only one element in feature2_list "
910  "that matches with an element in feature1_list.";
911  }
912  }
913  }
914  EXPECT_TRUE(found_match);
915  }
916  // Every feature in feature1_list should be matched to a feature in
917  // feature2_list.
918  EXPECT_EQ(map_feature1_to_feature2->size(), feature1->size());
919 }
920 
921 void ComparePolytope(const ccd_pt_t* polytope1, const ccd_pt_t* polytope2,
922  ccd_real_t tol) {
923  // Build the mapping between the vertices in polytope1 to the vertices in
924  // polytope2.
925  std::unordered_set<ccd_pt_vertex_t *> v1_set, v2_set;
926  std::unordered_map<ccd_pt_vertex_t*, ccd_pt_vertex_t*> map_v1_to_v2;
927  MapFeature1ToFeature2<ccd_pt_vertex_t>(
928  &polytope1->vertices, &polytope2->vertices,
929  [tol](const ccd_pt_vertex_t* v1, const ccd_pt_vertex_t* v2) {
930  return VertexPositionCoincide(v1, v2, tol);
931  },
932  &v1_set, &v2_set, &map_v1_to_v2);
933 
934  // Build the mapping between the edges in polytope1 to the edges in polytope2.
935  std::unordered_set<ccd_pt_edge_t *> e1_set, e2_set;
936  std::unordered_map<ccd_pt_edge_t*, ccd_pt_edge_t*> map_e1_to_e2;
937  MapFeature1ToFeature2<ccd_pt_edge_t>(
938  &polytope1->edges, &polytope2->edges,
939  [map_v1_to_v2](const ccd_pt_edge_t* e1, const ccd_pt_edge_t* e2) {
940  return EdgeMatch(e1, e2, map_v1_to_v2);
941  },
942  &e1_set, &e2_set, &map_e1_to_e2);
943 
944  // Build the mapping between the faces in polytope1 to the faces in polytope2.
945  std::unordered_set<ccd_pt_face_t *> f1_set, f2_set;
946  std::unordered_map<ccd_pt_face_t*, ccd_pt_face_t*> map_f1_to_f2;
947  MapFeature1ToFeature2<ccd_pt_face_t>(
948  &polytope1->faces, &polytope2->faces,
949  [map_e1_to_e2](const ccd_pt_face_t* f1, const ccd_pt_face_t* f2) {
950  return TriangleMatch(f1, f2, map_e1_to_e2);
951  },
952  &f1_set, &f2_set, &map_f1_to_f2);
953 
954  /* TODO(hongkai.dai@tri.global): enable the following check, when issue
955  https://github.com/danfis/libccd/issues/46 has been fixed. Currently
956  ccd_pt_vertex_t.edges are garbage.
957  // Now make sure that the edges connected to a vertex in polytope 1, are the
958  // same edges connected to the corresponding vertex in polytope 2.
959  for (const auto& v1 : v1_set) {
960  auto v2 = map_v1_to_v2[v1];
961  std::unordered_set<ccd_pt_edge_t*> v1_edges, v2_edges;
962  ccd_pt_edge_t* e;
963  ccdListForEachEntry(&v1->edges, e, ccd_pt_edge_t, list) {
964  v1_edges.insert(e);
965  }
966  ccdListForEachEntry(&v2->edges, e, ccd_pt_edge_t, list) {
967  v2_edges.insert(e);
968  }
969  EXPECT_EQ(v1_edges.size(), v2_edges.size());
970  // Now check for each edge connecting to v1, the corresponding edge is
971  // connected to v2.
972  for (const auto& v1_e : v1_edges) {
973  auto it = map_e1_to_e2.find(v1_e);
974  EXPECT_NE(it, map_e1_to_e2.end()) {
975  auto v2_e = it->second;
976  EXPECT_NE(v2_edges.find(v2_e), v2_edges.end());
977  }
978  }*/
979 
980  // Make sure that the faces connected to each edge in polytope 1, are the same
981  // face connected to the corresponding face in polytope 2.
982  for (const auto& e1 : e1_set) {
983  auto e2 = map_e1_to_e2[e1];
984  ccd_pt_face_t* f2_expected[2];
985  for (int i = 0; i < 2; ++i) {
986  f2_expected[i] = map_f1_to_f2[e1->faces[i]];
987  }
988  EXPECT_TRUE(
989  (f2_expected[0] == e2->faces[0] && f2_expected[1] == e2->faces[1]) ||
990  (f2_expected[0] == e2->faces[1] && f2_expected[1] == e2->faces[0]));
991  }
992 }
993 
994 GTEST_TEST(FCL_GJK_EPA, expandPolytope_tetrahedron1) {
995  // Expand the equilateral tetrahedron by adding a point just outside one of
996  // the triangle face. That nearest triangle face will be deleted, and the
997  // three new faces will be added, by connecting the new vertex with the three
998  // vertices on the removed face.
999  EquilateralTetrahedron polytope(0, 0, -0.1);
1000  // nearest point is on the bottom triangle
1001  ccd_support_t newv;
1002  newv.v.v[0] = 0;
1003  newv.v.v[1] = 0;
1004  newv.v.v[2] = -0.2;
1005 
1006  const int result = libccd_extension::expandPolytope(
1007  &polytope.polytope(), reinterpret_cast<ccd_pt_el_t*>(&polytope.f(0)),
1008  &newv);
1009  EXPECT_EQ(result, 0);
1010 
1011  // Construct the expanded polytope manually.
1012  EquilateralTetrahedron tetrahedron(0, 0, -0.1);
1013  ccd_pt_t& polytope_expected = tetrahedron.polytope();
1014  // The bottom face is removed.
1015  ccdPtDelFace(&polytope_expected, &tetrahedron.f(0));
1016  // Insert the vertex.
1017  ccd_pt_vertex_t* new_vertex =
1018  ccdPtAddVertexCoords(&polytope_expected, 0, 0, -0.2);
1019  // Add new edges.
1020  ccd_pt_edge_t* new_edges[3];
1021  for (int i = 0; i < 3; ++i) {
1022  new_edges[i] =
1023  ccdPtAddEdge(&polytope_expected, new_vertex, &tetrahedron.v(i));
1024  }
1025  // Add new faces.
1026  ccdPtAddFace(&polytope_expected, &tetrahedron.e(0), new_edges[0],
1027  new_edges[1]);
1028  ccdPtAddFace(&polytope_expected, &tetrahedron.e(1), new_edges[1],
1029  new_edges[2]);
1030  ccdPtAddFace(&polytope_expected, &tetrahedron.e(2), new_edges[2],
1031  new_edges[0]);
1032 
1033  ComparePolytope(&polytope.polytope(), &polytope_expected,
1035 }
1036 
1037 GTEST_TEST(FCL_GJK_EPA, expandPolytope_tetrahedron_2visible_faces) {
1038  // Expand the equilateral tetrahedron by adding a point just outside one edge.
1039  // The two neighbouring faces of that edge will be deleted. Four new faces
1040  // will be added, by connecting the new vertex with the remaining vertex on
1041  // the two removed faces, that is opposite to the removed edge.
1042  EquilateralTetrahedron polytope(0, 0, -0.1);
1043  // nearest point is on the bottom triangle
1044  ccd_support_t newv;
1045  newv.v.v[0] = 0;
1046  newv.v.v[1] = -0.5 / std::sqrt(3) - 0.1;
1047  newv.v.v[2] = -0.2;
1048 
1049  const int result = libccd_extension::expandPolytope(
1050  &polytope.polytope(), reinterpret_cast<ccd_pt_el_t*>(&polytope.e(0)),
1051  &newv);
1052  EXPECT_EQ(result, 0);
1053 
1054  // Construct the expanded polytope manually.
1055  EquilateralTetrahedron tetrahedron(0, 0, -0.1);
1056  ccd_pt_t& polytope_expected = tetrahedron.polytope();
1057  // The bottom face is removed.
1058  ccdPtDelFace(&polytope_expected, &tetrahedron.f(0));
1059  // The other face that neighbours with f(0) is removed.
1060  ccdPtDelFace(&polytope_expected, &tetrahedron.f(1));
1061  // The nearest edge is removed.
1062  ccdPtDelEdge(&polytope_expected, &tetrahedron.e(0));
1063  // Insert the vertex.
1064  ccd_pt_vertex_t* new_vertex = ccdPtAddVertexCoords(
1065  &polytope_expected, newv.v.v[0], newv.v.v[1], newv.v.v[2]);
1066  // Add new edges.
1067  ccd_pt_edge_t* new_edges[4];
1068  new_edges[0] =
1069  ccdPtAddEdge(&polytope_expected, new_vertex, &tetrahedron.v(0));
1070  new_edges[1] =
1071  ccdPtAddEdge(&polytope_expected, new_vertex, &tetrahedron.v(1));
1072  new_edges[2] =
1073  ccdPtAddEdge(&polytope_expected, new_vertex, &tetrahedron.v(2));
1074  new_edges[3] =
1075  ccdPtAddEdge(&polytope_expected, new_vertex, &tetrahedron.v(3));
1076  // Add new faces.
1077  ccdPtAddFace(&polytope_expected, &tetrahedron.e(3), new_edges[0],
1078  new_edges[3]);
1079  ccdPtAddFace(&polytope_expected, &tetrahedron.e(2), new_edges[0],
1080  new_edges[2]);
1081  ccdPtAddFace(&polytope_expected, &tetrahedron.e(4), new_edges[1],
1082  new_edges[3]);
1083  ccdPtAddFace(&polytope_expected, &tetrahedron.e(1), new_edges[1],
1084  new_edges[2]);
1085 
1086  ComparePolytope(&polytope.polytope(), &polytope_expected,
1088 }
1089 
1090 GTEST_TEST(FCL_GJK_EPA, expandPolytope_hexagram_1visible_face) {
1091  // Expand the Hexagram by adding a point just above the upper triangle.
1092  // The upper triangle will be deleted. Three new faces will be added, by
1093  // connecting the new vertex with the three vertices of the removed triangle.
1094  Hexagram hex(0, 0, -0.9);
1095  // nearest point is on the top triangle
1096  ccd_support_t newv;
1097  newv.v.v[0] = 0;
1098  newv.v.v[1] = 0;
1099  newv.v.v[2] = 0.2;
1100 
1101  const int result = libccd_extension::expandPolytope(
1102  &hex.polytope(), reinterpret_cast<ccd_pt_el_t*>(&hex.f(0)), &newv);
1103  EXPECT_EQ(result, 0);
1104 
1105  // Construct the expanded polytope manually.
1106  Hexagram hex_duplicate(0, 0, -0.9);
1107  ccd_pt_t& polytope_expected = hex_duplicate.polytope();
1108  // Remove the upper triangle.
1109  ccdPtDelFace(&polytope_expected, &hex_duplicate.f(0));
1110 
1111  // Add the new vertex.
1112  ccd_pt_vertex_t* new_vertex = ccdPtAddVertexCoords(
1113  &polytope_expected, newv.v.v[0], newv.v.v[1], newv.v.v[2]);
1114  // Add the new edges.
1115  ccd_pt_edge_t* new_edges[3];
1116  new_edges[0] =
1117  ccdPtAddEdge(&polytope_expected, new_vertex, &hex_duplicate.v(0));
1118  new_edges[1] =
1119  ccdPtAddEdge(&polytope_expected, new_vertex, &hex_duplicate.v(2));
1120  new_edges[2] =
1121  ccdPtAddEdge(&polytope_expected, new_vertex, &hex_duplicate.v(4));
1122  // Add the new faces.
1123  ccdPtAddFace(&polytope_expected, new_edges[0], new_edges[1],
1124  &hex_duplicate.e(0));
1125  ccdPtAddFace(&polytope_expected, new_edges[1], new_edges[2],
1126  &hex_duplicate.e(1));
1127  ccdPtAddFace(&polytope_expected, new_edges[2], new_edges[0],
1128  &hex_duplicate.e(2));
1129 
1130  ComparePolytope(&hex.polytope(), &polytope_expected,
1132 }
1133 
1134 GTEST_TEST(FCL_GJK_EPA, expandPolytope_hexagram_4_visible_faces) {
1135  // Expand the Hexagram by adding a point above the upper triangle by a certain
1136  // height, such that the new vertex can see the upper triangle, together with
1137  // the three triangles on the side of the hexagram. All these four triangles
1138  // will be removed. 6 new faces will be added by connecting the new vertex,
1139  // with eah vertex in the old hexagram.
1140  Hexagram hex(0, 0, -0.9);
1141  // nearest point is on the top triangle
1142  ccd_support_t newv;
1143  newv.v.v[0] = 0;
1144  newv.v.v[1] = 0;
1145  newv.v.v[2] = 1.2;
1146 
1147  const int result = libccd_extension::expandPolytope(
1148  &hex.polytope(), reinterpret_cast<ccd_pt_el_t*>(&hex.f(0)), &newv);
1149  EXPECT_EQ(result, 0);
1150 
1151  // Construct the expanded polytope manually.
1152  Hexagram hex_duplicate(0, 0, -0.9);
1153  ccd_pt_t& polytope_expected = hex_duplicate.polytope();
1154  // Remove the upper triangle.
1155  ccdPtDelFace(&polytope_expected, &hex_duplicate.f(0));
1156  // Remove the triangles on the side, which consists of two vertices on the
1157  // upper triangle, and one vertex on the lower triangle.
1158  ccdPtDelFace(&polytope_expected, &hex_duplicate.f(2));
1159  ccdPtDelFace(&polytope_expected, &hex_duplicate.f(4));
1160  ccdPtDelFace(&polytope_expected, &hex_duplicate.f(6));
1161  // Remove the edges of the upper triangle.
1162  ccdPtDelEdge(&polytope_expected, &hex_duplicate.e(0));
1163  ccdPtDelEdge(&polytope_expected, &hex_duplicate.e(1));
1164  ccdPtDelEdge(&polytope_expected, &hex_duplicate.e(2));
1165 
1166  // Add the new vertex.
1167  ccd_pt_vertex_t* new_vertex = ccdPtAddVertexCoords(
1168  &polytope_expected, newv.v.v[0], newv.v.v[1], newv.v.v[2]);
1169  // Add the new edges.
1170  ccd_pt_edge_t* new_edges[6];
1171  for (int i = 0; i < 6; ++i) {
1172  new_edges[i] =
1173  ccdPtAddEdge(&polytope_expected, new_vertex, &hex_duplicate.v(i));
1174  }
1175  // Add the new faces.
1176  for (int i = 0; i < 6; ++i) {
1177  ccdPtAddFace(&polytope_expected, new_edges[i % 6], new_edges[(i + 1) % 6],
1178  &hex_duplicate.e(i + 6));
1179  }
1180  ComparePolytope(&hex.polytope(), &polytope_expected,
1182 }
1183 
1184 GTEST_TEST(FCL_GJK_EPA, expandPolytope_error) {
1186  ccd_support_t newv;
1187 
1188  // Error condition 1: expanding with the nearest feature being a vertex.
1191  &tet.polytope(), reinterpret_cast<ccd_pt_el_t*>(&tet.v(0)), &newv),
1193  ".*expandPolytope.*The visible feature is a vertex.*");
1194 
1195  // Error condition 2: feature is edge, and vertex lies on the edge.
1196  ccd_vec3_t nearest;
1197  ccdVec3Copy(&nearest, &tet.v(0).v.v);
1198  ccdVec3Add(&nearest, &tet.v(1).v.v);
1199  ccdVec3Scale(&nearest, 0.5);
1200  newv.v = nearest;
1203  &tet.polytope(), reinterpret_cast<ccd_pt_el_t*>(&tet.e(0)), &newv),
1205  ".*expandPolytope.* nearest point and the new vertex are on an edge.*");
1206 }
1207 
1208 void CompareCcdVec3(const ccd_vec3_t& v, const ccd_vec3_t& v_expected,
1209  ccd_real_t tol) {
1210  for (int i = 0; i < 3; ++i) {
1211  EXPECT_NEAR(v.v[i], v_expected.v[i], tol);
1212  }
1213 }
1214 
1215 GTEST_TEST(FCL_GJK_EPA, penEPAPosClosest_vertex) {
1216  // The nearest point is a vertex on the polytope.
1217  // tetrahedron.v(0) is the origin.
1218  EquilateralTetrahedron tetrahedron(-0.5, 0.5 / std::sqrt(3), 0);
1219  // Make sure that v1 - v2 = v.
1220  tetrahedron.v(0).v.v1.v[0] = 1;
1221  tetrahedron.v(0).v.v1.v[1] = 2;
1222  tetrahedron.v(0).v.v1.v[2] = 3;
1223  for (int i = 0; i < 3; ++i) {
1224  tetrahedron.v(0).v.v2.v[i] = tetrahedron.v(0).v.v1.v[i];
1225  }
1226  ccd_vec3_t p1, p2;
1227  EXPECT_EQ(
1229  reinterpret_cast<const ccd_pt_el_t*>(&tetrahedron.v(0)), &p1, &p2),
1230  0);
1231  CompareCcdVec3(p1, tetrahedron.v(0).v.v1, constants<ccd_real_t>::eps_78());
1232  CompareCcdVec3(p2, tetrahedron.v(0).v.v2, constants<ccd_real_t>::eps_78());
1233 }
1234 
1235 GTEST_TEST(FCL_GJK_EPA, penEPAPosClosest_edge) {
1236  // The nearest point is on an edge of the polytope.
1237  // tetrahedron.e(1) contains the origin.
1238  EquilateralTetrahedron tetrahedron(0.25, -0.25 / std::sqrt(3), 0);
1239  // e(1) connects two vertices v(1) and v(2), make sure that v(1).v1 - v(1).v2
1240  // = v(1).v, also v(2).v1 - v(2).v2 = v(2).v
1241  ccdVec3Set(&tetrahedron.v(1).v.v1, 1, 2, 3);
1242  ccdVec3Set(&tetrahedron.v(2).v.v1, 4, 5, 6);
1243  for (int i = 1; i <= 2; ++i) {
1244  for (int j = 0; j < 3; ++j) {
1245  tetrahedron.v(i).v.v2.v[j] =
1246  tetrahedron.v(i).v.v1.v[j] - tetrahedron.v(i).v.v.v[j];
1247  }
1248  }
1249  // Notice that origin = 0.5*v(1).v + 0.5*v(2).v
1250  // So p1 = 0.5*v(1).v1 + 0.5*v(2).v1
1251  // p2 = 0.5*v(1).v2 + 0.5*v(2).v2
1252  ccd_vec3_t p1, p2;
1253  EXPECT_EQ(
1255  reinterpret_cast<const ccd_pt_el_t*>(&tetrahedron.e(1)), &p1, &p2),
1256  0);
1257  ccd_vec3_t p1_expected, p2_expected;
1258  ccdVec3Copy(&p1_expected, &tetrahedron.v(1).v.v1);
1259  ccdVec3Add(&p1_expected, &tetrahedron.v(2).v.v1);
1260  ccdVec3Scale(&p1_expected, ccd_real_t(0.5));
1261  ccdVec3Copy(&p2_expected, &tetrahedron.v(1).v.v2);
1262  ccdVec3Add(&p2_expected, &tetrahedron.v(2).v.v2);
1263  ccdVec3Scale(&p2_expected, ccd_real_t(0.5));
1264 
1265  CompareCcdVec3(p1, p1_expected, constants<ccd_real_t>::eps_78());
1266  CompareCcdVec3(p2, p2_expected, constants<ccd_real_t>::eps_78());
1267 }
1268 
1269 GTEST_TEST(FCL_GJK_EPA, penEPAPosClosest_face) {
1270  // The nearest point is on a face of the polytope, It is the center of
1271  // tetrahedron.f(1).
1272  const Vector3<ccd_real_t> bottom_center_pos =
1273  Vector3<ccd_real_t>(0, 1.0 / (3 * std::sqrt(3)), -std::sqrt(6) / 9) +
1274  0.01 * Vector3<ccd_real_t>(0, -2 * std::sqrt(2) / 3, 1.0 / 3);
1275  EquilateralTetrahedron tetrahedron(bottom_center_pos(0), bottom_center_pos(1),
1276  bottom_center_pos(2));
1277  // Assign v(i).v1 and v(i).v2 for i = 0, 1, 3, such that
1278  // v(i).v = v(i).v1 - v(i).v2
1279  tetrahedron.v(0).v.v1.v[0] = 1;
1280  tetrahedron.v(0).v.v1.v[1] = 2;
1281  tetrahedron.v(0).v.v1.v[2] = 3;
1282  tetrahedron.v(1).v.v1.v[0] = 4;
1283  tetrahedron.v(1).v.v1.v[1] = 5;
1284  tetrahedron.v(1).v.v1.v[2] = 6;
1285  tetrahedron.v(3).v.v1.v[0] = 7;
1286  tetrahedron.v(3).v.v1.v[1] = 8;
1287  tetrahedron.v(3).v.v1.v[2] = 9;
1288  for (int i : {0, 1, 3}) {
1289  for (int j = 0; j < 3; ++j) {
1290  tetrahedron.v(i).v.v2.v[j] =
1291  tetrahedron.v(i).v.v1.v[j] - tetrahedron.v(i).v.v.v[j];
1292  }
1293  }
1294 
1295  ccd_vec3_t p1, p2;
1296  EXPECT_EQ(
1298  reinterpret_cast<const ccd_pt_el_t*>(&tetrahedron.f(1)), &p1, &p2),
1299  0);
1300 
1301  // Notice that the nearest point = 1/3 * v(0).v + 1/3 * v(1).v + 1/3 * v(3).v
1302  // So p1 = 1/3 * (v(0).v1 + v(1).v1 + v(3).v1)
1303  // p2 = 1/3 * (v(0).v2 + v(1).v2 + v(3).v2)
1304  ccd_vec3_t p1_expected, p2_expected;
1305  ccdVec3Copy(&p1_expected, &tetrahedron.v(0).v.v1);
1306  ccdVec3Add(&p1_expected, &tetrahedron.v(1).v.v1);
1307  ccdVec3Add(&p1_expected, &tetrahedron.v(3).v.v1);
1308  ccdVec3Scale(&p1_expected, ccd_real_t(1.0 / 3));
1309  ccdVec3Copy(&p2_expected, &tetrahedron.v(0).v.v2);
1310  ccdVec3Add(&p2_expected, &tetrahedron.v(1).v.v2);
1311  ccdVec3Add(&p2_expected, &tetrahedron.v(3).v.v2);
1312  ccdVec3Scale(&p2_expected, ccd_real_t(1.0 / 3));
1313 
1314  CompareCcdVec3(p1, p1_expected, constants<ccd_real_t>::eps_78());
1315  CompareCcdVec3(p2, p2_expected, constants<ccd_real_t>::eps_78());
1316 }
1317 
1318 // Test convert2SimplexToTetrahedron function.
1319 // We construct a test scenario that two boxes are on the xy plane of frame F.
1320 // The two boxes penetrate to each other, as shown in the bottom plot.
1321 // y
1322 // ┏━━━│━━━┓Box1
1323 // ┏┃━━┓ ┃
1324 // ───┃┃──┃O───┃─────x
1325 // box2┗┃━━┛│ ┃
1326 // ┗━━━│━━━┛
1327 //
1328 // @param[in] X_WF The pose of the frame F measured and expressed in the world
1329 // frame W.
1330 // @param[out] o1 Box1
1331 // @param[out] o2 Box2
1332 // @param[out] ccd The ccd solver info.
1333 // @param[out] X_FB1 The pose of box 1 frame measured and expressed in frame F.
1334 // @param[out] X_FB2 The pose of box 2 frame measured and expressed in frame F.
1335 template <typename S>
1336 void SetUpBoxToBox(const Transform3<S>& X_WF, void** o1, void** o2, ccd_t* ccd,
1337  fcl::Transform3<S>* X_FB1, fcl::Transform3<S>* X_FB2) {
1338  const fcl::Vector3<S> box1_size(2, 2, 2);
1339  const fcl::Vector3<S> box2_size(1, 1, 2);
1340  // Box 1 is fixed.
1341  X_FB1->setIdentity();
1342  X_FB1->translation() << 0, 0, 1;
1343  X_FB2->setIdentity();
1344  X_FB2->translation() << -0.6, 0, 1;
1345 
1346  const fcl::Transform3<S> X_WB1 = X_WF * (*X_FB1);
1347  const fcl::Transform3<S> X_WB2 = X_WF * (*X_FB2);
1348  fcl::Box<S> box1(box1_size);
1349  fcl::Box<S> box2(box2_size);
1350  *o1 = GJKInitializer<S, fcl::Box<S>>::createGJKObject(box1, X_WB1);
1351  *o2 = GJKInitializer<S, fcl::Box<S>>::createGJKObject(box2, X_WB2);
1352 
1353  // Set up ccd solver.
1354  CCD_INIT(ccd);
1355  ccd->support1 = detail::GJKInitializer<S, Box<S>>::getSupportFunction();
1356  ccd->support2 = detail::GJKInitializer<S, Box<S>>::getSupportFunction();
1357  ccd->max_iterations = 1000;
1358  ccd->dist_tolerance = 1E-6;
1359 }
1360 
1361 template <typename S>
1362 Vector3<S> ToEigenVector(const ccd_vec3_t& v) {
1363  return (Vector3<S>() << v.v[0], v.v[1], v.v[2]).finished();
1364 }
1365 
1366 template <typename S>
1367 ccd_vec3_t ToCcdVec3(const Eigen::Ref<const Vector3<S>>& v) {
1368  ccd_vec3_t u;
1369  u.v[0] = v(0);
1370  u.v[1] = v(1);
1371  u.v[2] = v(2);
1372  return u;
1373 }
1374 
1375 template <typename S>
1377  void* o1 = NULL;
1378  void* o2 = NULL;
1379  ccd_t ccd;
1380  fcl::Transform3<S> X_FB1, X_FB2;
1381  SetUpBoxToBox(X_WF, &o1, &o2, &ccd, &X_FB1, &X_FB2);
1382 
1383  // Construct a 2-simplex that contains the origin. The vertices of this
1384  // 2-simplex are on the boundary of the Minkowski difference.
1385  ccd_simplex_t simplex;
1386  ccdSimplexInit(&simplex);
1387  ccd_support_t pts[3];
1388  // We find three points Pa1, Pb1, Pc1 on box 1, and three points Pa2, Pb2, Pc2
1389  // on box 2, such that the 2-simplex with vertices (Pa1 - Pa2, Pb1 - Pb2,
1390  // Pc1 - Pc2) contains the origin.
1391  const Vector3<S> p_FPa1(-1, -1, 0.1);
1392  const Vector3<S> p_FPa2(-0.1, 0.5, 0.1);
1393  pts[0].v = ToCcdVec3<S>(p_FPa1 - p_FPa2);
1394  pts[0].v1 = ToCcdVec3<S>(p_FPa1);
1395  pts[0].v2 = ToCcdVec3<S>(p_FPa2);
1396 
1397  const Vector3<S> p_FPb1(-1, 1, 0.1);
1398  const Vector3<S> p_FPb2(-0.1, 0.5, 0.1);
1399  pts[1].v = ToCcdVec3<S>(p_FPb1 - p_FPb2);
1400  pts[1].v1 = ToCcdVec3<S>(p_FPb1);
1401  pts[1].v2 = ToCcdVec3<S>(p_FPb2);
1402 
1403  const Vector3<S> p_FPc1(1, 1, 0.1);
1404  const Vector3<S> p_FPc2(-0.1, 0.5, 0.1);
1405  pts[2].v = ToCcdVec3<S>(p_FPc1 - p_FPc2);
1406  pts[2].v1 = ToCcdVec3<S>(p_FPc1);
1407  pts[2].v2 = ToCcdVec3<S>(p_FPc2);
1408  for (int i = 0; i < 3; ++i) {
1409  ccdSimplexAdd(&simplex, &pts[i]);
1410  }
1411  // Make sure that the origin is on the triangle.
1412  const Vector3<S> a = ToEigenVector<S>(pts[0].v);
1413  const Vector3<S> b = ToEigenVector<S>(pts[1].v);
1414  const Vector3<S> c = ToEigenVector<S>(pts[2].v);
1415  // We first check if the origin is co-planar with vertices a, b, and c.
1416  // If a, b, c and origin are co-planar, then aᵀ · (b × c)) = 0
1417  EXPECT_NEAR(a.dot(b.cross(c)), 0, 1E-10);
1418  // Now check if origin is within the triangle, by checking the condition
1419  // (a × b)ᵀ · (b × c) ≥ 0
1420  // (b × c)ᵀ · (c × a) ≥ 0
1421  // (c × a)ᵀ · (a × b) ≥ 0
1422  // Namely the cross product a × b, b × c, c × a all point to the same
1423  // direction.
1424  // Note the check above is valid when either a, b, c is a zero vector, or
1425  // they are co-linear.
1426  EXPECT_GE(a.cross(b).dot(b.cross(c)), 0);
1427  EXPECT_GE(b.cross(c).dot(c.cross(a)), 0);
1428  EXPECT_GE(c.cross(a).dot(a.cross(b)), 0);
1429 
1430  ccd_pt_t polytope;
1431  ccdPtInit(&polytope);
1432  ccd_pt_el_t* nearest{};
1433  libccd_extension::convert2SimplexToTetrahedron(o1, o2, &ccd, &simplex,
1434  &polytope, &nearest);
1435  // Box1 and Box2 are not touching, so nearest is set to null.
1436  EXPECT_EQ(nearest, nullptr);
1437 
1438  // Check the polytope
1439  // The polytope should have 4 vertices, with three of them being the vertices
1440  // of the 2-simplex, and another vertex that has the maximal support along
1441  // the normal directions of the 2-simplex.
1442  // We first construct the set containing the polytope vertices.
1443  std::unordered_set<ccd_pt_vertex_t*> polytope_vertices;
1444  {
1445  ccd_pt_vertex_t* v;
1446  ccdListForEachEntry(&polytope.vertices, v, ccd_pt_vertex_t, list) {
1447  const auto it = polytope_vertices.find(v);
1448  EXPECT_EQ(it, polytope_vertices.end());
1449  polytope_vertices.emplace_hint(it, v);
1450  }
1451  }
1452  EXPECT_EQ(polytope_vertices.size(), 4u);
1453  // We need to find out the vertex on the polytope, that is not the vertex
1454  // of the simplex.
1455  ccd_pt_vertex_t* non_simplex_vertex{nullptr};
1456  // A simplex vertex matches with a polytope vertex if they coincide.
1457  int num_matched_vertices = 0;
1458  for (const auto& v : polytope_vertices) {
1459  bool found_match = false;
1460  for (int i = 0; i < 3; ++i) {
1461  if (ccdVec3Dist2(&v->v.v, &pts[i].v) < constants<S>::eps_78()) {
1462  num_matched_vertices++;
1463  found_match = true;
1464  break;
1465  }
1466  }
1467  if (!found_match) {
1468  non_simplex_vertex = v;
1469  }
1470  }
1471  EXPECT_EQ(num_matched_vertices, 3);
1472  EXPECT_NE(non_simplex_vertex, nullptr);
1473  // Make sure that the non-simplex vertex has the maximal support along the
1474  // simplex normal direction.
1475  // Find the two normal directions of the 2-simplex.
1476  Vector3<S> dir1, dir2;
1477  Vector3<S> ab, ac;
1478  ab = ToEigenVector<S>(pts[1].v) - ToEigenVector<S>(pts[0].v);
1479  ac = ToEigenVector<S>(pts[2].v) - ToEigenVector<S>(pts[0].v);
1480  dir1 = ab.cross(ac);
1481  dir2 = -dir1;
1482  // Now make sure non_simplex_vertex has the largest support
1483  // p_B1V1 are the position of the box 1 vertices in box1 frame B1.
1484  // p_B2V1 are the position of the box 2 vertices in box2 frame B2.
1485  const Vector3<S> box1_size{2, 2, 2};
1486  const Vector3<S> box2_size{1, 1, 2};
1487  Eigen::Matrix<S, 3, 8> p_B1V1, p_B2V2;
1488  p_B1V1 << -1, -1, -1, -1, 1, 1, 1, 1, -1, -1, 1, 1, -1, -1, 1, 1, -1, 1, -1,
1489  1, -1, 1, -1, 1;
1490  p_B2V2 << -1, -1, -1, -1, 1, 1, 1, 1, -1, -1, 1, 1, -1, -1, 1, 1, -1, 1, -1,
1491  1, -1, 1, -1, 1;
1492  for (int i = 0; i < 3; ++i) {
1493  p_B1V1.row(i) *= box1_size[i] / 2;
1494  p_B2V2.row(i) *= box2_size[i] / 2;
1495  }
1496 
1497  const Eigen::Matrix<S, 3, 8> p_FV1 = X_FB1 * p_B1V1;
1498  const Eigen::Matrix<S, 3, 8> p_FV2 = X_FB2 * p_B2V2;
1499  // The support of the Minkowski difference along direction dir1.
1500  const S max_support1 = (dir1.transpose() * p_FV1).maxCoeff() -
1501  (dir1.transpose() * p_FV2).minCoeff();
1502  // The support of the Minkowski difference along direction dir2.
1503  const S max_support2 = (dir2.transpose() * p_FV1).maxCoeff() -
1504  (dir2.transpose() * p_FV2).minCoeff();
1505 
1506  const double expected_max_support = std::max(max_support1, max_support2);
1507  const double non_simplex_vertex_support1 =
1508  ToEigenVector<S>(non_simplex_vertex->v.v).dot(dir1);
1509  const double non_simplex_vertex_support2 =
1510  ToEigenVector<S>(non_simplex_vertex->v.v).dot(dir2);
1511  EXPECT_NEAR(
1512  std::max(non_simplex_vertex_support1, non_simplex_vertex_support2),
1513  expected_max_support, constants<ccd_real_t>::eps_78());
1514 
1515  // Also make sure the non_simplex_vertex actually is inside the Minkowski
1516  // difference.
1517  // Call the non-simplex vertex as D. This vertex equals to the difference
1518  // between a point Dv1 in box1, and a point Dv2 in box2.
1519  const Vector3<S> p_B1Dv1 =
1520  (X_WF * X_FB1).inverse() * ToEigenVector<S>(non_simplex_vertex->v.v1);
1521  const Vector3<S> p_B2Dv2 =
1522  (X_WF * X_FB2).inverse() * ToEigenVector<S>(non_simplex_vertex->v.v2);
1523  // Now check if p_B1Dv1 is in box1, and p_B2Dv2 is in box2.
1524  for (int i = 0; i < 3; ++i) {
1525  EXPECT_LE(p_B1Dv1(i), box1_size(i) / 2 + constants<ccd_real_t>::eps_78());
1526  EXPECT_LE(p_B2Dv2(i), box2_size(i) / 2 + constants<ccd_real_t>::eps_78());
1527  EXPECT_GE(p_B1Dv1(i), -box1_size(i) / 2 - constants<ccd_real_t>::eps_78());
1528  EXPECT_GE(p_B2Dv2(i), -box2_size(i) / 2 - constants<ccd_real_t>::eps_78());
1529  }
1530  // Now that we make sure the vertices of the polytope is correct, we will
1531  // check the edges and faces of the polytope. We do so by constructing an
1532  // expected polytope, and compare it with the polytope obtained from
1533  // convert2SimplexToTetrahedron().
1534  ccd_pt_t polytope_expected;
1535  ccdPtInit(&polytope_expected);
1536 
1537  ccd_pt_vertex_t* vertices_expected[4];
1538  int v_count = 0;
1539  for (const auto& v : polytope_vertices) {
1540  vertices_expected[v_count++] = ccdPtAddVertex(&polytope_expected, &v->v);
1541  }
1542  ccd_pt_edge_t* edges_expected[6];
1543  edges_expected[0] = ccdPtAddEdge(&polytope_expected, vertices_expected[0],
1544  vertices_expected[1]);
1545  edges_expected[1] = ccdPtAddEdge(&polytope_expected, vertices_expected[1],
1546  vertices_expected[2]);
1547  edges_expected[2] = ccdPtAddEdge(&polytope_expected, vertices_expected[2],
1548  vertices_expected[0]);
1549  edges_expected[3] = ccdPtAddEdge(&polytope_expected, vertices_expected[3],
1550  vertices_expected[0]);
1551  edges_expected[4] = ccdPtAddEdge(&polytope_expected, vertices_expected[3],
1552  vertices_expected[1]);
1553  edges_expected[5] = ccdPtAddEdge(&polytope_expected, vertices_expected[3],
1554  vertices_expected[2]);
1555 
1556  ccdPtAddFace(&polytope_expected, edges_expected[0], edges_expected[3],
1557  edges_expected[4]);
1558  ccdPtAddFace(&polytope_expected, edges_expected[1], edges_expected[4],
1559  edges_expected[5]);
1560  ccdPtAddFace(&polytope_expected, edges_expected[2], edges_expected[3],
1561  edges_expected[5]);
1562  ccdPtAddFace(&polytope_expected, edges_expected[0], edges_expected[1],
1563  edges_expected[2]);
1564 
1565  ComparePolytope(&polytope, &polytope_expected, constants<S>::eps_34());
1566 
1567  ccdPtDestroy(&polytope_expected);
1568  ccdPtDestroy(&polytope);
1569 }
1570 
1571 template <typename S>
1573  Transform3<S> X_WF;
1574  X_WF.setIdentity();
1576 
1577  X_WF.translation() << 0, 0, 1;
1579 
1580  X_WF.translation() << -0.2, 0.4, 0.1;
1582 }
1583 
1585  TestSimplexToPolytope3<double>();
1586  TestSimplexToPolytope3<float>();
1587 }
1588 
1589 } // namespace detail
1590 } // namespace fcl
1591 
1592 //==============================================================================
1593 int main(int argc, char* argv[]) {
1594  ::testing::InitGoogleTest(&argc, argv);
1595  return RUN_ALL_TESTS();
1596 }
ccdPtAddFace
ccd_pt_face_t * ccdPtAddFace(ccd_pt_t *pt, ccd_pt_edge_t *e1, ccd_pt_edge_t *e2, ccd_pt_edge_t *e3)
FCL_EXPECT_THROWS_MESSAGE
#define FCL_EXPECT_THROWS_MESSAGE(expression, exception, regexp)
Definition: expect_throws_message.h:111
fcl::detail::Polytope::polytope
const ccd_pt_t & polytope() const
Definition: test_gjk_libccd-inl_epa.cpp:83
_ccd_pt_face_t::edge
__CCD_PT_EL ccd_pt_edge_t * edge[3]
Reference to surrounding edges.
Definition: polytope.h:84
fcl::detail::ToEigenVector
Vector3< S > ToEigenVector(const ccd_vec3_t &v)
Definition: test_gjk_libccd-inl_epa.cpp:1362
_ccd_pt_vertex_t
Definition: polytope.h:56
EXPECT_GE
#define EXPECT_GE(val1, val2)
Definition: gtest.h:1987
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
std::tr1::f2
const T1 const T2 & f2
Definition: gtest-tuple.h:680
fcl::detail::libccd_extension::ComputeVisiblePatchRecursiveSanityCheck
static bool ComputeVisiblePatchRecursiveSanityCheck(const ccd_pt_t &polytope, const std::unordered_set< ccd_pt_edge_t * > &border_edges, const std::unordered_set< ccd_pt_face_t * > &visible_faces, const std::unordered_set< ccd_pt_edge_t * > &internal_edges)
Definition: gjk_libccd-inl.h:1115
EXPECT_LE
#define EXPECT_LE(val1, val2)
Definition: gtest.h:1983
ccdPtAddEdge
ccd_pt_edge_t * ccdPtAddEdge(ccd_pt_t *pt, ccd_pt_vertex_t *v1, ccd_pt_vertex_t *v2)
fcl::detail::Polytope::polytope
ccd_pt_t & polytope()
Definition: test_gjk_libccd-inl_epa.cpp:75
_ccd_pt_t::faces
ccd_list_t faces
List of faces.
Definition: polytope.h:95
fcl::detail::TestSimplexToPolytope3
void TestSimplexToPolytope3()
Definition: test_gjk_libccd-inl_epa.cpp:1572
ccdSimplexAdd
_ccd_inline void ccdSimplexAdd(ccd_simplex_t *s, const ccd_support_t *v)
Definition: simplex.h:74
fcl::detail::libccd_extension::penEPAPosClosest
static int penEPAPosClosest(const ccd_pt_el_t *nearest, ccd_vec3_t *p1, ccd_vec3_t *p2)
Definition: gjk_libccd-inl.h:2131
fcl::detail::ToCcdVec3
ccd_vec3_t ToCcdVec3(const Eigen::Ref< const Vector3< S >> &v)
Definition: test_gjk_libccd-inl_epa.cpp:1367
fcl::detail::Polytope
Definition: test_gjk_libccd-inl_epa.cpp:55
gtest.h
fcl::detail::TestSimplexToPolytope3InGivenFrame
void TestSimplexToPolytope3InGivenFrame(const Transform3< S > &X_WF)
Definition: test_gjk_libccd-inl_epa.cpp:1376
fcl::detail::Polytope::e
std::vector< ccd_pt_edge_t * > & e()
Definition: test_gjk_libccd-inl_epa.cpp:87
_ccd_support_t
Definition: support.h:27
ccdPtInit
void ccdPtInit(ccd_pt_t *pt)
fcl::detail::EquilateralTetrahedron
Definition: test_gjk_libccd-inl_epa.cpp:183
fcl::detail::CompareCcdVec3
void CompareCcdVec3(const ccd_vec3_t &v, const ccd_vec3_t &v_expected, ccd_real_t tol)
Definition: test_gjk_libccd-inl_epa.cpp:1208
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
fcl::detail::Polytope::e_
std::vector< ccd_pt_edge_t * > e_
Definition: test_gjk_libccd-inl_epa.cpp:92
EXPECT_NE
#define EXPECT_NE(expected, actual)
Definition: gtest.h:1981
_ccd_support_t::v
ccd_vec3_t v
Support point in minkowski sum.
Definition: support.h:43
FCL_EXPECT_THROWS_MESSAGE_IF_DEBUG
#define FCL_EXPECT_THROWS_MESSAGE_IF_DEBUG(expression, exception, regexp)
Definition: expect_throws_message.h:133
fcl::detail::MapFeature1ToFeature2
void MapFeature1ToFeature2(const ccd_list_t *feature1_list, const ccd_list_t *feature2_list, std::function< bool(const T *, const T *)> cmp_feature, std::unordered_set< T * > *feature1, std::unordered_set< T * > *feature2, std::unordered_map< T *, T * > *map_feature1_to_feature2)
Definition: test_gjk_libccd-inl_epa.cpp:880
main
int main(int argc, char *argv[])
Definition: test_gjk_libccd-inl_epa.cpp:1593
fcl::detail::Polytope::f
const ccd_pt_face_t & f(int i) const
Definition: test_gjk_libccd-inl_epa.cpp:81
max
static T max(T x, T y)
Definition: svm.cpp:52
fcl::detail::CheckComputeVisiblePatchRecursive
void CheckComputeVisiblePatchRecursive(const Polytope &polytope, ccd_pt_face_t &face, const std::vector< int > &edge_indices, const ccd_vec3_t &new_vertex, const std::unordered_set< int > &border_edge_indices_expected, const std::unordered_set< int > &visible_face_indices_expected, const std::unordered_set< int > &internal_edges_indices_expected)
Definition: test_gjk_libccd-inl_epa.cpp:586
fcl::detail::Tetrahedron
Definition: test_gjk_libccd-inl_epa.cpp:103
fcl::detail::GJKInitializer
initialize GJK stuffs
Definition: gjk_libccd.h:76
EXPECT_TRUE
#define EXPECT_TRUE(condition)
Definition: gtest.h:1914
fcl::detail::CheckComputeVisiblePatchColinearNewVertex
void CheckComputeVisiblePatchColinearNewVertex(EquilateralTetrahedron &tet, double rho)
Definition: test_gjk_libccd-inl_epa.cpp:712
_ccd_support_t::v1
ccd_vec3_t v1
Support point in obj1.
Definition: support.h:44
expect_throws_message.h
ccdListForEachEntry
#define ccdListForEachEntry(head, pos, postype, member)
Definition: list.h:66
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::detail::Polytope::v
const ccd_pt_vertex_t & v(int i) const
Definition: test_gjk_libccd-inl_epa.cpp:77
EXPECT_EQ
#define EXPECT_EQ(expected, actual)
Definition: gtest.h:1977
_ccd_support_t::v2
ccd_vec3_t v2
Support point in obj2.
Definition: support.h:45
GTEST_FAIL
#define GTEST_FAIL()
Definition: gtest.h:1872
polytope.h
fcl::detail::Polytope::v
std::vector< ccd_pt_vertex_t * > & v()
Definition: test_gjk_libccd-inl_epa.cpp:86
fcl::detail::TetrahedronColinearVertices
std::array< Vector3< ccd_real_t >, 4 > TetrahedronColinearVertices()
Definition: test_gjk_libccd-inl_epa.cpp:146
fcl::detail::Polytope::e
const ccd_pt_edge_t & e(int i) const
Definition: test_gjk_libccd-inl_epa.cpp:79
gjk_libccd-inl.h
_ccd_simplex_t
Definition: simplex.h:28
fcl::detail::EquilateralTetrahedronVertices
std::array< fcl::Vector3< ccd_real_t >, 4 > EquilateralTetrahedronVertices(ccd_real_t bottom_center_x, ccd_real_t bottom_center_y, ccd_real_t bottom_center_z, ccd_real_t edge_length)
Definition: test_gjk_libccd-inl_epa.cpp:127
EXPECT_FALSE
#define EXPECT_FALSE(condition)
Definition: gtest.h:1917
fcl::detail::CheckComputeVisiblePatch
void CheckComputeVisiblePatch(const Polytope &polytope, ccd_pt_face_t &face, const ccd_vec3_t &new_vertex, const std::unordered_set< int > &border_edge_indices_expected, const std::unordered_set< int > &visible_face_indices_expected, const std::unordered_set< int > &internal_edges_indices_expected)
Definition: test_gjk_libccd-inl_epa.cpp:613
fcl::detail::libccd_extension::supportEPADirection
static ccd_vec3_t supportEPADirection(const ccd_pt_t *polytope, const ccd_pt_el_t *nearest_feature)
Definition: gjk_libccd-inl.h:1489
RUN_ALL_TESTS
int RUN_ALL_TESTS() GTEST_MUST_USE_RESULT_
Definition: gtest.h:2287
fcl::detail::Polytope::e
ccd_pt_edge_t & e(int i)
Definition: test_gjk_libccd-inl_epa.cpp:71
_ccd_list_t
Definition: list.h:28
_ccd_pt_edge_t
Definition: polytope.h:68
fcl::detail::Tetrahedron::Tetrahedron
Tetrahedron(const std::array< fcl::Vector3< ccd_real_t >, 4 > &vertices)
Definition: test_gjk_libccd-inl_epa.cpp:105
_ccd_pt_el_t
Definition: polytope.h:45
_ccd_pt_face_t
Definition: polytope.h:81
ccdSimplexInit
_ccd_inline void ccdSimplexInit(ccd_simplex_t *s)
Definition: simplex.h:49
fcl::detail::Polytope::f
ccd_pt_face_t & f(int i)
Definition: test_gjk_libccd-inl_epa.cpp:73
fcl::detail::Polytope::Polytope
Polytope()
Definition: test_gjk_libccd-inl_epa.cpp:57
fcl::detail::IsElementInSet
bool IsElementInSet(const std::unordered_set< T * > &S, const T *element)
Definition: test_gjk_libccd-inl_epa.cpp:551
ccdPtDestroy
void ccdPtDestroy(ccd_pt_t *pt)
ccdPtDelEdge
_ccd_inline int ccdPtDelEdge(ccd_pt_t *pt, ccd_pt_edge_t *)
Definition: polytope.h:204
fcl::detail::Polytope::v_
std::vector< ccd_pt_vertex_t * > v_
Definition: test_gjk_libccd-inl_epa.cpp:91
testing::InitGoogleTest
GTEST_API_ void InitGoogleTest(int *argc, char **argv)
Definition: gtest.cc:5008
fcl::detail::GTEST_TEST
GTEST_TEST(FCL_GJK_EPA, faceNormalPointingOutward)
Definition: test_gjk_libccd-inl_epa.cpp:205
fcl::detail::CheckTetrahedronFaceNormal
void CheckTetrahedronFaceNormal(const Tetrahedron &p)
Definition: test_gjk_libccd-inl_epa.cpp:193
ccdPtAddVertexCoords
_ccd_inline ccd_pt_vertex_t * ccdPtAddVertexCoords(ccd_pt_t *pt, ccd_real_t x, ccd_real_t y, ccd_real_t z)
Definition: polytope.h:179
fcl::detail::libccd_extension::ComputeVisiblePatchRecursive
static void ComputeVisiblePatchRecursive(const ccd_pt_t &polytope, ccd_pt_face_t &f, int edge_index, const ccd_vec3_t &query_point, std::unordered_set< ccd_pt_edge_t * > *border_edges, std::unordered_set< ccd_pt_face_t * > *visible_faces, std::unordered_set< ccd_pt_face_t * > *hidden_faces, std::unordered_set< ccd_pt_edge_t * > *internal_edges)
Definition: gjk_libccd-inl.h:1212
fcl::detail::Hexagram
Definition: test_gjk_libccd-inl_epa.cpp:497
fcl::detail::libccd_extension::convert2SimplexToTetrahedron
static int convert2SimplexToTetrahedron(const void *obj1, const void *obj2, const ccd_t *ccd, const ccd_simplex_t *simplex, ccd_pt_t *polytope, ccd_pt_el_t **nearest)
Definition: gjk_libccd-inl.h:735
ccdPtDelFace
_ccd_inline int ccdPtDelFace(ccd_pt_t *pt, ccd_pt_face_t *)
Definition: polytope.h:226
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:50
ccd_pt_t
struct _ccd_pt_t ccd_pt_t
Definition: polytope.h:101
fcl::detail::VertexPositionCoincide
bool VertexPositionCoincide(const ccd_pt_vertex_t *v1, const ccd_pt_vertex_t *v2, ccd_real_t tol)
Definition: test_gjk_libccd-inl_epa.cpp:819
fcl::constants::pi
static constexpr S pi()
The mathematical constant pi.
Definition: constants.h:134
fcl::detail::CheckComputeVisiblePatchCommon
void CheckComputeVisiblePatchCommon(const Polytope &polytope, const std::unordered_set< ccd_pt_edge_t * > &border_edges, const std::unordered_set< ccd_pt_face_t * > &visible_faces, const std::unordered_set< ccd_pt_edge_t * > &internal_edges, const std::unordered_set< int > &border_edge_indices_expected, const std::unordered_set< int > &visible_face_indices_expected, const std::unordered_set< int > internal_edges_indices_expected)
Definition: test_gjk_libccd-inl_epa.cpp:558
fcl::detail::ComparePolytope
void ComparePolytope(const ccd_pt_t *polytope1, const ccd_pt_t *polytope2, ccd_real_t tol)
Definition: test_gjk_libccd-inl_epa.cpp:921
fcl::detail::libccd_extension::faceNormalPointingOutward
static ccd_vec3_t faceNormalPointingOutward(const ccd_pt_t *polytope, const ccd_pt_face_t *face)
Definition: gjk_libccd-inl.h:982
fcl::detail::EdgeMatch
bool EdgeMatch(const ccd_pt_edge_t *e1, const ccd_pt_edge_t *e2, const std::unordered_map< ccd_pt_vertex_t *, ccd_pt_vertex_t * > &map_v1_to_v2)
Definition: test_gjk_libccd-inl_epa.cpp:826
fcl::detail::libccd_extension::isOutsidePolytopeFace
static bool isOutsidePolytopeFace(const ccd_pt_t *polytope, const ccd_pt_face_t *f, const ccd_vec3_t *pt)
Definition: gjk_libccd-inl.h:1092
fcl::detail::TetrahedronSmallFaceVertices
std::array< Vector3< ccd_real_t >, 4 > TetrahedronSmallFaceVertices()
Definition: test_gjk_libccd-inl_epa.cpp:155
ccdPtAddVertex
ccd_pt_vertex_t * ccdPtAddVertex(ccd_pt_t *pt, const ccd_support_t *v)
EXPECT_THROW
#define EXPECT_THROW(statement, expected_exception)
Definition: gtest.h:1898
fcl::detail::Polytope::~Polytope
~Polytope()
Definition: test_gjk_libccd-inl_epa.cpp:62
_ccd_pt_vertex_t::v
ccd_support_t v
Definition: polytope.h:60
fcl::detail::SetUpBoxToBox
void SetUpBoxToBox(const Transform3< S > &X_WF, void **o1, void **o2, ccd_t *ccd, fcl::Transform3< S > *X_FB1, fcl::Transform3< S > *X_FB2)
Definition: test_gjk_libccd-inl_epa.cpp:1336
fcl::detail::Polytope::f_
std::vector< ccd_pt_face_t * > f_
Definition: test_gjk_libccd-inl_epa.cpp:93
fcl::constants
Definition: constants.h:129
fcl::detail::Hexagram::Hexagram
Hexagram(ccd_real_t bottom_center_x=0, ccd_real_t bottom_center_y=0, ccd_real_t bottom_center_z=0)
Definition: test_gjk_libccd-inl_epa.cpp:499
_ccd_pt_t
Definition: polytope.h:92
fcl::detail::EquilateralTetrahedron::EquilateralTetrahedron
EquilateralTetrahedron(ccd_real_t bottom_center_x=0, ccd_real_t bottom_center_y=0, ccd_real_t bottom_center_z=0, ccd_real_t edge_length=1)
Definition: test_gjk_libccd-inl_epa.cpp:185
fcl::detail::Polytope::polytope_
ccd_pt_t * polytope_
Definition: test_gjk_libccd-inl_epa.cpp:94
fcl::detail::Polytope::v
ccd_pt_vertex_t & v(int i)
Definition: test_gjk_libccd-inl_epa.cpp:69
fcl::detail::Polytope::f
std::vector< ccd_pt_face_t * > & f()
Definition: test_gjk_libccd-inl_epa.cpp:88
std::tr1::f1
const T1 & f1
Definition: gtest-tuple.h:675
fcl::detail::TriangleMatch
bool TriangleMatch(const ccd_pt_face_t *f1, const ccd_pt_face_t *f2, const std::unordered_map< ccd_pt_edge_t *, ccd_pt_edge_t * > &map_e1_to_e2)
Definition: test_gjk_libccd-inl_epa.cpp:844
fcl::detail::libccd_extension::ComputeVisiblePatch
static void ComputeVisiblePatch(const ccd_pt_t &polytope, ccd_pt_face_t &f, const ccd_vec3_t &query_point, std::unordered_set< ccd_pt_edge_t * > *border_edges, std::unordered_set< ccd_pt_face_t * > *visible_faces, std::unordered_set< ccd_pt_edge_t * > *internal_edges)
Definition: gjk_libccd-inl.h:1315
_ccd_pt_t::edges
ccd_list_t edges
List of edges.
Definition: polytope.h:94
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
_ccd_pt_edge_t::faces
struct _ccd_pt_face_t * faces[2]
Reference to faces.
Definition: polytope.h:72
fcl::detail::libccd_extension::expandPolytope
static int expandPolytope(ccd_pt_t *polytope, ccd_pt_el_t *el, const ccd_support_t *newv)
Definition: gjk_libccd-inl.h:1371
fcl::detail::FailedAtThisConfiguration
Definition: failed_at_this_configuration.h:64
_ccd_pt_t::vertices
ccd_list_t vertices
List of vertices.
Definition: polytope.h:93
_ccd_pt_edge_t::vertex
__CCD_PT_EL ccd_pt_vertex_t * vertex[2]
Reference to vertices.
Definition: polytope.h:71


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