src/narrowphase/gjk.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2015, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #include <hpp/fcl/narrowphase/gjk.h>
39 #include <hpp/fcl/internal/intersect.h>
40 #include <hpp/fcl/internal/tools.h>
41 #include <hpp/fcl/shape/geometric_shapes_traits.h>
42 
43 namespace hpp {
44 namespace fcl {
45 
46 namespace details {
47 
48 void getShapeSupport(const TriangleP* triangle, const Vec3f& dir,
49  Vec3f& support, int&, MinkowskiDiff::ShapeData*) {
50  FCL_REAL dota = dir.dot(triangle->a);
51  FCL_REAL dotb = dir.dot(triangle->b);
52  FCL_REAL dotc = dir.dot(triangle->c);
53  if (dota > dotb) {
54  if (dotc > dota)
55  support = triangle->c;
56  else
57  support = triangle->a;
58  } else {
59  if (dotc > dotb)
60  support = triangle->c;
61  else
62  support = triangle->b;
63  }
64 }
65 
66 inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support,
67  int&, MinkowskiDiff::ShapeData*) {
68  const FCL_REAL inflate = (dir.array() == 0).any() ? 1.00000001 : 1.;
69  support.noalias() =
70  (dir.array() > 0)
71  .select(inflate * box->halfSide, -inflate * box->halfSide);
72 }
73 
74 inline void getShapeSupport(const Sphere*, const Vec3f& /*dir*/, Vec3f& support,
75  int&, MinkowskiDiff::ShapeData*) {
76  support.setZero();
77 }
78 
79 inline void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir,
80  Vec3f& support, int&, MinkowskiDiff::ShapeData*) {
81  FCL_REAL a2 = ellipsoid->radii[0] * ellipsoid->radii[0];
82  FCL_REAL b2 = ellipsoid->radii[1] * ellipsoid->radii[1];
83  FCL_REAL c2 = ellipsoid->radii[2] * ellipsoid->radii[2];
84 
85  Vec3f v(a2 * dir[0], b2 * dir[1], c2 * dir[2]);
86 
87  FCL_REAL d = std::sqrt(v.dot(dir));
88 
89  support = v / d;
90 }
91 
92 inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir,
93  Vec3f& support, int&, MinkowskiDiff::ShapeData*) {
94  support.head<2>().setZero();
95  if (dir[2] > 0)
96  support[2] = capsule->halfLength;
97  else
98  support[2] = -capsule->halfLength;
99 }
100 
101 void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, int&,
102  MinkowskiDiff::ShapeData*) {
103  // The cone radius is, for -h < z < h, (h - z) * r / (2*h)
104  static const FCL_REAL inflate = 1.00001;
105  FCL_REAL h = cone->halfLength;
106  FCL_REAL r = cone->radius;
107 
108  if (dir.head<2>().isZero()) {
109  support.head<2>().setZero();
110  if (dir[2] > 0)
111  support[2] = h;
112  else
113  support[2] = -inflate * h;
114  return;
115  }
116  FCL_REAL zdist = dir[0] * dir[0] + dir[1] * dir[1];
117  FCL_REAL len = zdist + dir[2] * dir[2];
118  zdist = std::sqrt(zdist);
119 
120  if (dir[2] <= 0) {
121  FCL_REAL rad = r / zdist;
122  support.head<2>() = rad * dir.head<2>();
123  support[2] = -h;
124  return;
125  }
126 
127  len = std::sqrt(len);
128  FCL_REAL sin_a = r / std::sqrt(r * r + 4 * h * h);
129 
130  if (dir[2] > len * sin_a)
131  support << 0, 0, h;
132  else {
133  FCL_REAL rad = r / zdist;
134  support.head<2>() = rad * dir.head<2>();
135  support[2] = -h;
136  }
137 }
138 
139 void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support,
140  int&, MinkowskiDiff::ShapeData*) {
141  // The inflation makes the object look strictly convex to GJK and EPA. This
142  // helps solving particular cases (e.g. a cylinder with itself at the same
143  // position...)
144  static const FCL_REAL inflate = 1.00001;
145  FCL_REAL half_h = cylinder->halfLength;
146  FCL_REAL r = cylinder->radius;
147 
148  if (dir.head<2>() == Eigen::Matrix<FCL_REAL, 2, 1>::Zero()) half_h *= inflate;
149 
150  if (dir[2] > 0)
151  support[2] = half_h;
152  else if (dir[2] < 0)
153  support[2] = -half_h;
154  else {
155  support[2] = 0;
156  r *= inflate;
157  }
158  if (dir.head<2>() == Eigen::Matrix<FCL_REAL, 2, 1>::Zero())
159  support.head<2>().setZero();
160  else
161  support.head<2>() = dir.head<2>().normalized() * r;
162  assert(fabs(support[0] * dir[1] - support[1] * dir[0]) <
163  sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
164 }
165 
166 struct SmallConvex : ShapeBase {};
167 struct LargeConvex : ShapeBase {};
168 
169 void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir,
170  Vec3f& support, int& hint,
171  MinkowskiDiff::ShapeData* data) {
172  assert(data != NULL);
173 
174  const Vec3f* pts = convex->points;
175  const ConvexBase::Neighbors* nn = convex->neighbors;
176 
177  if (hint < 0 || hint >= (int)convex->num_points) hint = 0;
178  FCL_REAL maxdot = pts[hint].dot(dir);
179  std::vector<int8_t>& visited = data->visited;
180  visited.assign(convex->num_points, false);
181  visited[static_cast<std::size_t>(hint)] = true;
182  // when the first face is orthogonal to dir, all the dot products will be
183  // equal. Yet, the neighbors must be visited.
184  bool found = true, loose_check = true;
185  while (found) {
186  const ConvexBase::Neighbors& n = nn[hint];
187  found = false;
188  for (int in = 0; in < n.count(); ++in) {
189  const unsigned int ip = n[in];
190  if (visited[ip]) continue;
191  visited[ip] = true;
192  const FCL_REAL dot = pts[ip].dot(dir);
193  bool better = false;
194  if (dot > maxdot) {
195  better = true;
196  loose_check = false;
197  } else if (loose_check && dot == maxdot)
198  better = true;
199  if (better) {
200  maxdot = dot;
201  hint = static_cast<int>(ip);
202  found = true;
203  }
204  }
205  }
206 
207  support = pts[hint];
208 }
209 
210 void getShapeSupportLinear(const ConvexBase* convex, const Vec3f& dir,
211  Vec3f& support, int& hint,
212  MinkowskiDiff::ShapeData*) {
213  const Vec3f* pts = convex->points;
214 
215  hint = 0;
216  FCL_REAL maxdot = pts[0].dot(dir);
217  for (int i = 1; i < (int)convex->num_points; ++i) {
218  FCL_REAL dot = pts[i].dot(dir);
219  if (dot > maxdot) {
220  maxdot = dot;
221  hint = i;
222  }
223  }
224  support = pts[hint];
225 }
226 
227 void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support,
228  int& hint, MinkowskiDiff::ShapeData*) {
229  // TODO add benchmark to set a proper value for switching between linear and
230  // logarithmic.
231  if (convex->num_points > 32) {
232  MinkowskiDiff::ShapeData data;
233  getShapeSupportLog(convex, dir, support, hint, &data);
234  } else
235  getShapeSupportLinear(convex, dir, support, hint, NULL);
236 }
237 
238 inline void getShapeSupport(const SmallConvex* convex, const Vec3f& dir,
239  Vec3f& support, int& hint,
240  MinkowskiDiff::ShapeData* data) {
241  getShapeSupportLinear(reinterpret_cast<const ConvexBase*>(convex), dir,
242  support, hint, data);
243 }
244 
245 inline void getShapeSupport(const LargeConvex* convex, const Vec3f& dir,
246  Vec3f& support, int& hint,
247  MinkowskiDiff::ShapeData* data) {
248  getShapeSupportLog(reinterpret_cast<const ConvexBase*>(convex), dir, support,
249  hint, data);
250 }
251 
252 #define CALL_GET_SHAPE_SUPPORT(ShapeType) \
253  getShapeSupport( \
254  static_cast<const ShapeType*>(shape), \
255  (shape_traits<ShapeType>::NeedNormalizedDir && !dirIsNormalized) \
256  ? dir.normalized() \
257  : dir, \
258  support, hint, NULL)
259 
260 Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized,
261  int& hint) {
262  Vec3f support;
263  switch (shape->getNodeType()) {
264  case GEOM_TRIANGLE:
265  CALL_GET_SHAPE_SUPPORT(TriangleP);
266  break;
267  case GEOM_BOX:
268  CALL_GET_SHAPE_SUPPORT(Box);
269  break;
270  case GEOM_SPHERE:
271  CALL_GET_SHAPE_SUPPORT(Sphere);
272  break;
273  case GEOM_ELLIPSOID:
274  CALL_GET_SHAPE_SUPPORT(Ellipsoid);
275  break;
276  case GEOM_CAPSULE:
277  CALL_GET_SHAPE_SUPPORT(Capsule);
278  break;
279  case GEOM_CONE:
280  CALL_GET_SHAPE_SUPPORT(Cone);
281  break;
282  case GEOM_CYLINDER:
283  CALL_GET_SHAPE_SUPPORT(Cylinder);
284  break;
285  case GEOM_CONVEX:
286  CALL_GET_SHAPE_SUPPORT(ConvexBase);
287  break;
288  case GEOM_PLANE:
289  case GEOM_HALFSPACE:
290  default:
291  support.setZero();
292  ; // nothing
293  }
294 
295  return support;
296 }
297 
298 #undef CALL_GET_SHAPE_SUPPORT
299 
300 template <typename Shape0, typename Shape1, bool TransformIsIdentity>
301 void getSupportTpl(const Shape0* s0, const Shape1* s1, const Matrix3f& oR1,
302  const Vec3f& ot1, const Vec3f& dir, Vec3f& support0,
303  Vec3f& support1, support_func_guess_t& hint,
304  MinkowskiDiff::ShapeData data[2]) {
305  getShapeSupport(s0, dir, support0, hint[0], &(data[0]));
306  if (TransformIsIdentity)
307  getShapeSupport(s1, -dir, support1, hint[1], &(data[1]));
308  else {
309  getShapeSupport(s1, -oR1.transpose() * dir, support1, hint[1], &(data[1]));
310  support1 = oR1 * support1 + ot1;
311  }
312 }
313 
314 template <typename Shape0, typename Shape1, bool TransformIsIdentity>
315 void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3f& dir,
316  bool dirIsNormalized, Vec3f& support0, Vec3f& support1,
317  support_func_guess_t& hint,
318  MinkowskiDiff::ShapeData data[2]) {
319  enum {
320  NeedNormalizedDir = bool((bool)shape_traits<Shape0>::NeedNormalizedDir ||
321  (bool)shape_traits<Shape1>::NeedNormalizedDir)
322  };
323 #ifndef NDEBUG
324  // Need normalized direction and direction is normalized
325  assert(!NeedNormalizedDir || !dirIsNormalized ||
326  fabs(dir.squaredNorm() - 1) < 1e-6);
327  // Need normalized direction but direction is not normalized.
328  assert(!NeedNormalizedDir || dirIsNormalized ||
329  fabs(dir.normalized().squaredNorm() - 1) < 1e-6);
330  // Don't need normalized direction. Check that dir is not zero.
331  assert(NeedNormalizedDir || dir.cwiseAbs().maxCoeff() >= 1e-6);
332 #endif
333  getSupportTpl<Shape0, Shape1, TransformIsIdentity>(
334  static_cast<const Shape0*>(md.shapes[0]),
335  static_cast<const Shape1*>(md.shapes[1]), md.oR1, md.ot1,
336  (NeedNormalizedDir && !dirIsNormalized) ? dir.normalized() : dir,
337  support0, support1, hint, data);
338 }
339 
340 template <typename Shape0>
341 MinkowskiDiff::GetSupportFunction makeGetSupportFunction1(
342  const ShapeBase* s1, bool identity, Eigen::Array<FCL_REAL, 1, 2>& inflation,
343  int linear_log_convex_threshold) {
344  inflation[1] = 0;
345  switch (s1->getNodeType()) {
346  case GEOM_TRIANGLE:
347  if (identity)
348  return getSupportFuncTpl<Shape0, TriangleP, true>;
349  else
350  return getSupportFuncTpl<Shape0, TriangleP, false>;
351  case GEOM_BOX:
352  if (identity)
353  return getSupportFuncTpl<Shape0, Box, true>;
354  else
355  return getSupportFuncTpl<Shape0, Box, false>;
356  case GEOM_SPHERE:
357  inflation[1] = static_cast<const Sphere*>(s1)->radius;
358  if (identity)
359  return getSupportFuncTpl<Shape0, Sphere, true>;
360  else
361  return getSupportFuncTpl<Shape0, Sphere, false>;
362  case GEOM_ELLIPSOID:
363  if (identity)
364  return getSupportFuncTpl<Shape0, Ellipsoid, true>;
365  else
366  return getSupportFuncTpl<Shape0, Ellipsoid, false>;
367  case GEOM_CAPSULE:
368  inflation[1] = static_cast<const Capsule*>(s1)->radius;
369  if (identity)
370  return getSupportFuncTpl<Shape0, Capsule, true>;
371  else
372  return getSupportFuncTpl<Shape0, Capsule, false>;
373  case GEOM_CONE:
374  if (identity)
375  return getSupportFuncTpl<Shape0, Cone, true>;
376  else
377  return getSupportFuncTpl<Shape0, Cone, false>;
378  case GEOM_CYLINDER:
379  if (identity)
380  return getSupportFuncTpl<Shape0, Cylinder, true>;
381  else
382  return getSupportFuncTpl<Shape0, Cylinder, false>;
383  case GEOM_CONVEX:
384  if ((int)static_cast<const ConvexBase*>(s1)->num_points >
385  linear_log_convex_threshold) {
386  if (identity)
387  return getSupportFuncTpl<Shape0, LargeConvex, true>;
388  else
389  return getSupportFuncTpl<Shape0, LargeConvex, false>;
390  } else {
391  if (identity)
392  return getSupportFuncTpl<Shape0, SmallConvex, true>;
393  else
394  return getSupportFuncTpl<Shape0, SmallConvex, false>;
395  }
396  default:
397  throw std::logic_error("Unsupported geometric shape");
398  }
399 }
400 
401 MinkowskiDiff::GetSupportFunction makeGetSupportFunction0(
402  const ShapeBase* s0, const ShapeBase* s1, bool identity,
403  Eigen::Array<FCL_REAL, 1, 2>& inflation, int linear_log_convex_threshold) {
404  inflation[0] = 0;
405  switch (s0->getNodeType()) {
406  case GEOM_TRIANGLE:
407  return makeGetSupportFunction1<TriangleP>(s1, identity, inflation,
408  linear_log_convex_threshold);
409  break;
410  case GEOM_BOX:
411  return makeGetSupportFunction1<Box>(s1, identity, inflation,
412  linear_log_convex_threshold);
413  break;
414  case GEOM_SPHERE:
415  inflation[0] = static_cast<const Sphere*>(s0)->radius;
416  return makeGetSupportFunction1<Sphere>(s1, identity, inflation,
417  linear_log_convex_threshold);
418  break;
419  case GEOM_ELLIPSOID:
420  return makeGetSupportFunction1<Ellipsoid>(s1, identity, inflation,
421  linear_log_convex_threshold);
422  break;
423  case GEOM_CAPSULE:
424  inflation[0] = static_cast<const Capsule*>(s0)->radius;
425  return makeGetSupportFunction1<Capsule>(s1, identity, inflation,
426  linear_log_convex_threshold);
427  break;
428  case GEOM_CONE:
429  return makeGetSupportFunction1<Cone>(s1, identity, inflation,
430  linear_log_convex_threshold);
431  break;
432  case GEOM_CYLINDER:
433  return makeGetSupportFunction1<Cylinder>(s1, identity, inflation,
434  linear_log_convex_threshold);
435  break;
436  case GEOM_CONVEX:
437  if ((int)static_cast<const ConvexBase*>(s0)->num_points >
438  linear_log_convex_threshold)
439  return makeGetSupportFunction1<LargeConvex>(
440  s1, identity, inflation, linear_log_convex_threshold);
441  else
442  return makeGetSupportFunction1<SmallConvex>(
443  s1, identity, inflation, linear_log_convex_threshold);
444  break;
445  default:
446  throw std::logic_error("Unsupported geometric shape");
447  }
448 }
449 
450 bool getNormalizeSupportDirection(const ShapeBase* shape) {
451  switch (shape->getNodeType()) {
452  case GEOM_TRIANGLE:
453  return (bool)shape_traits<TriangleP>::NeedNesterovNormalizeHeuristic;
454  break;
455  case GEOM_BOX:
456  return (bool)shape_traits<Box>::NeedNesterovNormalizeHeuristic;
457  break;
458  case GEOM_SPHERE:
459  return (bool)shape_traits<Sphere>::NeedNesterovNormalizeHeuristic;
460  break;
461  case GEOM_ELLIPSOID:
462  return (bool)shape_traits<Ellipsoid>::NeedNesterovNormalizeHeuristic;
463  break;
464  case GEOM_CAPSULE:
465  return (bool)shape_traits<Capsule>::NeedNesterovNormalizeHeuristic;
466  break;
467  case GEOM_CONE:
468  return (bool)shape_traits<Cone>::NeedNesterovNormalizeHeuristic;
469  break;
470  case GEOM_CYLINDER:
471  return (bool)shape_traits<Cylinder>::NeedNesterovNormalizeHeuristic;
472  break;
473  case GEOM_CONVEX:
474  return (bool)shape_traits<ConvexBase>::NeedNesterovNormalizeHeuristic;
475  break;
476  default:
477  throw std::logic_error("Unsupported geometric shape");
478  }
479 }
480 
481 void getNormalizeSupportDirectionFromShapes(const ShapeBase* shape0,
482  const ShapeBase* shape1,
483  bool& normalize_support_direction) {
484  normalize_support_direction = getNormalizeSupportDirection(shape0) &&
485  getNormalizeSupportDirection(shape1);
486 }
487 
488 void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1,
489  const Transform3f& tf0, const Transform3f& tf1) {
490  shapes[0] = shape0;
491  shapes[1] = shape1;
492  getNormalizeSupportDirectionFromShapes(shape0, shape1,
493  normalize_support_direction);
494 
495  oR1.noalias() = tf0.getRotation().transpose() * tf1.getRotation();
496  ot1.noalias() = tf0.getRotation().transpose() *
497  (tf1.getTranslation() - tf0.getTranslation());
498 
499  bool identity = (oR1.isIdentity() && ot1.isZero());
500 
501  getSupportFunc = makeGetSupportFunction0(shape0, shape1, identity, inflation,
502  linear_log_convex_threshold);
503 }
504 
505 void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1) {
506  shapes[0] = shape0;
507  shapes[1] = shape1;
508  getNormalizeSupportDirectionFromShapes(shape0, shape1,
509  normalize_support_direction);
510 
511  oR1.setIdentity();
512  ot1.setZero();
513 
514  getSupportFunc = makeGetSupportFunction0(shape0, shape1, true, inflation,
515  linear_log_convex_threshold);
516 }
517 
518 void GJK::initialize() {
519  nfree = 0;
520  status = Failed;
521  distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();
522  simplex = NULL;
523  gjk_variant = GJKVariant::DefaultGJK;
524  convergence_criterion = GJKConvergenceCriterion::VDB;
525  convergence_criterion_type = GJKConvergenceCriterionType::Relative;
526 }
527 
528 Vec3f GJK::getGuessFromSimplex() const { return ray; }
529 
530 namespace details {
531 
532 bool getClosestPoints(const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1) {
533  GJK::SimplexV* const* vs = simplex.vertex;
534 
535  for (GJK::vertex_id_t i = 0; i < simplex.rank; ++i) {
536  assert(vs[i]->w.isApprox(vs[i]->w0 - vs[i]->w1));
537  }
538 
539  Project::ProjectResult projection;
540  switch (simplex.rank) {
541  case 1:
542  w0 = vs[0]->w0;
543  w1 = vs[0]->w1;
544  return true;
545  case 2: {
546  const Vec3f &a = vs[0]->w, a0 = vs[0]->w0, a1 = vs[0]->w1, b = vs[1]->w,
547  b0 = vs[1]->w0, b1 = vs[1]->w1;
548  FCL_REAL la, lb;
549  Vec3f N(b - a);
550  la = N.dot(-a);
551  if (la <= 0) {
552  assert(false);
553  w0 = a0;
554  w1 = a1;
555  } else {
556  lb = N.squaredNorm();
557  if (la > lb) {
558  assert(false);
559  w0 = b0;
560  w1 = b1;
561  } else {
562  lb = la / lb;
563  la = 1 - lb;
564  w0 = la * a0 + lb * b0;
565  w1 = la * a1 + lb * b1;
566  }
567  }
568  }
569  return true;
570  case 3:
571  // TODO avoid the reprojection
572  projection = Project::projectTriangleOrigin(vs[0]->w, vs[1]->w, vs[2]->w);
573  break;
574  case 4: // We are in collision.
575  projection = Project::projectTetrahedraOrigin(vs[0]->w, vs[1]->w,
576  vs[2]->w, vs[3]->w);
577  break;
578  default:
579  throw std::logic_error("The simplex rank must be in [ 1, 4 ]");
580  }
581  w0.setZero();
582  w1.setZero();
583  for (GJK::vertex_id_t i = 0; i < simplex.rank; ++i) {
584  w0 += projection.parameterization[i] * vs[i]->w0;
585  w1 += projection.parameterization[i] * vs[i]->w1;
586  }
587  return true;
588 }
589 
591 template <bool Separated>
592 void inflate(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) {
593  const Eigen::Array<FCL_REAL, 1, 2>& I(shape.inflation);
594  Eigen::Array<bool, 1, 2> inflate(I > 0);
595  if (!inflate.any()) return;
596  Vec3f w(w0 - w1);
597  FCL_REAL n2 = w.squaredNorm();
598  // TODO should be use a threshold (Eigen::NumTraits<FCL_REAL>::epsilon()) ?
599  if (n2 == 0.) {
600  if (inflate[0]) w0[0] += I[0] * (Separated ? -1 : 1);
601  if (inflate[1]) w1[0] += I[1] * (Separated ? 1 : -1);
602  return;
603  }
604 
605  w /= std::sqrt(n2);
606  if (Separated) {
607  if (inflate[0]) w0 -= I[0] * w;
608  if (inflate[1]) w1 += I[1] * w;
609  } else {
610  if (inflate[0]) w0 += I[0] * w;
611  if (inflate[1]) w1 -= I[1] * w;
612  }
613 }
614 
615 } // namespace details
616 
617 bool GJK::getClosestPoints(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) {
618  bool res = details::getClosestPoints(*simplex, w0, w1);
619  if (!res) return false;
620  details::inflate<true>(shape, w0, w1);
621  return true;
622 }
623 
624 GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
625  const support_func_guess_t& supportHint) {
626  FCL_REAL alpha = 0;
627  iterations = 0;
628  const FCL_REAL inflation = shape_.inflation.sum();
629  const FCL_REAL upper_bound = distance_upper_bound + inflation;
630 
631  free_v[0] = &store_v[0];
632  free_v[1] = &store_v[1];
633  free_v[2] = &store_v[2];
634  free_v[3] = &store_v[3];
635 
636  nfree = 4;
637  current = 0;
638  status = Valid;
639  shape = &shape_;
640  distance = 0.0;
641  simplices[0].rank = 0;
642  support_hint = supportHint;
643 
644  FCL_REAL rl = guess.norm();
645  if (rl < tolerance) {
646  ray = Vec3f(-1, 0, 0);
647  rl = 1;
648  } else
649  ray = guess;
650 
651  // Momentum
652  GJKVariant current_gjk_variant = gjk_variant;
653  Vec3f w = ray;
654  Vec3f dir = ray;
655  Vec3f y;
656  FCL_REAL momentum;
657  bool normalize_support_direction = shape->normalize_support_direction;
658  do {
659  vertex_id_t next = (vertex_id_t)(1 - current);
660  Simplex& curr_simplex = simplices[current];
661  Simplex& next_simplex = simplices[next];
662 
663  // check A: when origin is near the existing simplex, stop
664  // TODO this is an early stop which may cause the following issue.
665  // - EPA will not run correctly because it starts with a tetrahedron which
666  // does not include the origin. Note that, at this stage, we do not know
667  // whether a tetrahedron including the origin exists.
668  if (rl < tolerance) // mean origin is near the face of original simplex,
669  // return touch
670  {
671  assert(rl > 0);
672  status = Inside;
673  distance = -inflation; // should we take rl into account ?
674  break;
675  }
676 
677  // Compute direction for support call
678  switch (current_gjk_variant) {
679  case DefaultGJK:
680  dir = ray;
681  break;
682 
683  case NesterovAcceleration:
684  // Normalize heuristic for collision pairs involving convex but not
685  // strictly-convex shapes This corresponds to most use cases.
686  if (normalize_support_direction) {
687  momentum = (FCL_REAL(iterations) + 2) / (FCL_REAL(iterations) + 3);
688  y = momentum * ray + (1 - momentum) * w;
689  FCL_REAL y_norm = y.norm();
690  // ray is the point of the Minkowski difference which currently the
691  // closest to the origin. Therefore, y.norm() > ray.norm() Hence, if
692  // check A above has not stopped the algorithm, we necessarily have
693  // y.norm() > tolerance. The following assert is just a safety check.
694  assert(y_norm > tolerance);
695  dir = momentum * dir / dir.norm() + (1 - momentum) * y / y_norm;
696  } else {
697  momentum = (FCL_REAL(iterations) + 1) / (FCL_REAL(iterations) + 3);
698  y = momentum * ray + (1 - momentum) * w;
699  dir = momentum * dir + (1 - momentum) * y;
700  }
701  break;
702 
703  default:
704  throw std::logic_error("Invalid momentum variant.");
705  }
706 
707  appendVertex(curr_simplex, -dir, false,
708  support_hint); // see below, ray points away from origin
709 
710  // check removed (by ?): when the new support point is close to previous
711  // support points, stop (as the new simplex is degenerated)
712  w = curr_simplex.vertex[curr_simplex.rank - 1]->w;
713 
714  // check B: no collision if omega > 0
715  FCL_REAL omega = dir.dot(w) / dir.norm();
716  if (omega > upper_bound) {
717  distance = omega - inflation;
718  status = EarlyStopped;
719  break;
720  }
721 
722  // Check to remove acceleration
723  if (current_gjk_variant != DefaultGJK) {
724  FCL_REAL frank_wolfe_duality_gap = 2 * ray.dot(ray - w);
725  if (frank_wolfe_duality_gap - tolerance <= 0) {
726  removeVertex(simplices[current]);
727  current_gjk_variant = DefaultGJK; // move back to classic GJK
728  continue; // continue to next iteration
729  }
730  }
731 
732  // check C: when the new support point is close to the sub-simplex where the
733  // ray point lies, stop (as the new simplex again is degenerated)
734  bool cv_check_passed = checkConvergence(w, rl, alpha, omega);
735  // TODO here, we can stop at iteration 0 if this condition is met.
736  // We stopping at iteration 0, the closest point will not be valid.
737  // if(diff - tolerance * rl <= 0)
738  if (iterations > 0 && cv_check_passed) {
739  if (iterations > 0) removeVertex(simplices[current]);
740  if (current_gjk_variant != DefaultGJK) {
741  current_gjk_variant = DefaultGJK; // move back to classic GJK
742  continue;
743  }
744  distance = rl - inflation;
745  // TODO When inflation is strictly positive, the distance may be exactly
746  // zero (so the ray is not zero) and we are not in the case rl <
747  // tolerance.
748  if (distance < tolerance) status = Inside;
749  break;
750  }
751 
752  // This has been rewritten thanks to the excellent video:
753  // https://youtu.be/Qupqu1xe7Io
754  bool inside;
755  switch (curr_simplex.rank) {
756  case 1: // Only at the first iteration
757  assert(iterations == 0);
758  ray = w;
759  inside = false;
760  next_simplex.rank = 1;
761  next_simplex.vertex[0] = curr_simplex.vertex[0];
762  break;
763  case 2:
764  inside = projectLineOrigin(curr_simplex, next_simplex);
765  break;
766  case 3:
767  inside = projectTriangleOrigin(curr_simplex, next_simplex);
768  break;
769  case 4:
770  inside = projectTetrahedraOrigin(curr_simplex, next_simplex);
771  break;
772  default:
773  throw std::logic_error("Invalid simplex rank");
774  }
775  assert(nfree + next_simplex.rank == 4);
776  current = next;
777  if (!inside) rl = ray.norm();
778  if (inside || rl == 0) {
779  status = Inside;
780  distance = -inflation - 1.;
781  break;
782  }
783 
784  status = ((++iterations) < max_iterations) ? status : Failed;
785 
786  } while (status == Valid);
787 
788  simplex = &simplices[current];
789  assert(simplex->rank > 0 && simplex->rank < 5);
790  return status;
791 }
792 
793 bool GJK::checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha,
794  const FCL_REAL& omega) {
795  FCL_REAL diff;
796  bool check_passed;
797  // x^* is the optimal solution (projection of origin onto the Minkowski
798  // difference).
799  // x^k is the current iterate (x^k = `ray` in the code).
800  // Each criterion provides a different guarantee on the distance to the
801  // optimal solution.
802  switch (convergence_criterion) {
803  case VDB:
804  // alpha is the distance to the best separating hyperplane found so far
805  alpha = std::max(alpha, omega);
806  // ||x^*|| - ||x^k|| <= diff
807  diff = rl - alpha;
808  switch (convergence_criterion_type) {
809  case Absolute:
810  throw std::logic_error("VDB convergence criterion is relative.");
811  break;
812  case Relative:
813  check_passed = (diff - tolerance * rl) <= 0;
814  break;
815  default:
816  throw std::logic_error("Invalid convergence criterion type.");
817  }
818  break;
819 
820  case DualityGap:
821  // ||x^* - x^k||^2 <= diff
822  diff = 2 * ray.dot(ray - w);
823  switch (convergence_criterion_type) {
824  case Absolute:
825  check_passed = (diff - tolerance) <= 0;
826  break;
827  case Relative:
828  check_passed = ((diff / tolerance * rl) - tolerance * rl) <= 0;
829  break;
830  default:
831  throw std::logic_error("Invalid convergence criterion type.");
832  }
833  break;
834 
835  case Hybrid:
836  // alpha is the distance to the best separating hyperplane found so far
837  alpha = std::max(alpha, omega);
838  // ||x^* - x^k||^2 <= diff
839  diff = rl * rl - alpha * alpha;
840  switch (convergence_criterion_type) {
841  case Absolute:
842  check_passed = (diff - tolerance) <= 0;
843  break;
844  case Relative:
845  check_passed = ((diff / tolerance * rl) - tolerance * rl) <= 0;
846  break;
847  default:
848  throw std::logic_error("Invalid convergence criterion type.");
849  }
850  break;
851 
852  default:
853  throw std::logic_error("Invalid convergence criterion.");
854  }
855  return check_passed;
856 }
857 
858 inline void GJK::removeVertex(Simplex& simplex) {
859  free_v[nfree++] = simplex.vertex[--simplex.rank];
860 }
861 
862 inline void GJK::appendVertex(Simplex& simplex, const Vec3f& v,
863  bool isNormalized, support_func_guess_t& hint) {
864  simplex.vertex[simplex.rank] = free_v[--nfree]; // set the memory
865  getSupport(v, isNormalized, *simplex.vertex[simplex.rank++], hint);
866 }
867 
868 bool GJK::encloseOrigin() {
869  Vec3f axis(Vec3f::Zero());
870  support_func_guess_t hint = support_func_guess_t::Zero();
871  switch (simplex->rank) {
872  case 1:
873  for (int i = 0; i < 3; ++i) {
874  axis[i] = 1;
875  appendVertex(*simplex, axis, true, hint);
876  if (encloseOrigin()) return true;
877  removeVertex(*simplex);
878  axis[i] = -1;
879  appendVertex(*simplex, -axis, true, hint);
880  if (encloseOrigin()) return true;
881  removeVertex(*simplex);
882  axis[i] = 0;
883  }
884  break;
885  case 2: {
886  Vec3f d = simplex->vertex[1]->w - simplex->vertex[0]->w;
887  for (int i = 0; i < 3; ++i) {
888  axis[i] = 1;
889  Vec3f p = d.cross(axis);
890  if (!p.isZero()) {
891  appendVertex(*simplex, p, false, hint);
892  if (encloseOrigin()) return true;
893  removeVertex(*simplex);
894  appendVertex(*simplex, -p, false, hint);
895  if (encloseOrigin()) return true;
896  removeVertex(*simplex);
897  }
898  axis[i] = 0;
899  }
900  } break;
901  case 3:
902  axis.noalias() =
903  (simplex->vertex[1]->w - simplex->vertex[0]->w)
904  .cross(simplex->vertex[2]->w - simplex->vertex[0]->w);
905  if (!axis.isZero()) {
906  appendVertex(*simplex, axis, false, hint);
907  if (encloseOrigin()) return true;
908  removeVertex(*simplex);
909  appendVertex(*simplex, -axis, false, hint);
910  if (encloseOrigin()) return true;
911  removeVertex(*simplex);
912  }
913  break;
914  case 4:
915  if (std::abs(triple(simplex->vertex[0]->w - simplex->vertex[3]->w,
916  simplex->vertex[1]->w - simplex->vertex[3]->w,
917  simplex->vertex[2]->w - simplex->vertex[3]->w)) > 0)
918  return true;
919  break;
920  }
921 
922  return false;
923 }
924 
925 inline void originToPoint(const GJK::Simplex& current, GJK::vertex_id_t a,
926  const Vec3f& A, GJK::Simplex& next, Vec3f& ray) {
927  // A is the closest to the origin
928  ray = A;
929  next.vertex[0] = current.vertex[a];
930  next.rank = 1;
931 }
932 
933 inline void originToSegment(const GJK::Simplex& current, GJK::vertex_id_t a,
934  GJK::vertex_id_t b, const Vec3f& A, const Vec3f& B,
935  const Vec3f& AB, const FCL_REAL& ABdotAO,
936  GJK::Simplex& next, Vec3f& ray) {
937  // ray = - ( AB ^ AO ) ^ AB = (AB.B) A + (-AB.A) B
938  ray = AB.dot(B) * A + ABdotAO * B;
939 
940  next.vertex[0] = current.vertex[b];
941  next.vertex[1] = current.vertex[a];
942  next.rank = 2;
943 
944  // To ensure backward compatibility
945  ray /= AB.squaredNorm();
946 }
947 
948 inline bool originToTriangle(const GJK::Simplex& current, GJK::vertex_id_t a,
949  GJK::vertex_id_t b, GJK::vertex_id_t c,
950  const Vec3f& ABC, const FCL_REAL& ABCdotAO,
951  GJK::Simplex& next, Vec3f& ray) {
952  next.rank = 3;
953  next.vertex[2] = current.vertex[a];
954 
955  if (ABCdotAO == 0) {
956  next.vertex[0] = current.vertex[c];
957  next.vertex[1] = current.vertex[b];
958  ray.setZero();
959  return true;
960  }
961  if (ABCdotAO > 0) { // Above triangle
962  next.vertex[0] = current.vertex[c];
963  next.vertex[1] = current.vertex[b];
964  } else {
965  next.vertex[0] = current.vertex[b];
966  next.vertex[1] = current.vertex[c];
967  }
968 
969  // To ensure backward compatibility
970  ray = -ABCdotAO / ABC.squaredNorm() * ABC;
971  return false;
972 }
973 
974 bool GJK::projectLineOrigin(const Simplex& current, Simplex& next) {
975  const vertex_id_t a = 1, b = 0;
976  // A is the last point we added.
977  const Vec3f& A = current.vertex[a]->w;
978  const Vec3f& B = current.vertex[b]->w;
979 
980  const Vec3f AB = B - A;
981  const FCL_REAL d = AB.dot(-A);
982  assert(d <= AB.squaredNorm());
983 
984  if (d == 0) {
985  // Two extremely unlikely cases:
986  // - AB is orthogonal to A: should never happen because it means the support
987  // function did not do any progress and GJK should have stopped.
988  // - A == origin
989  // In any case, A is the closest to the origin
990  originToPoint(current, a, A, next, ray);
991  free_v[nfree++] = current.vertex[b];
992  return A.isZero();
993  } else if (d < 0) {
994  // A is the closest to the origin
995  originToPoint(current, a, A, next, ray);
996  free_v[nfree++] = current.vertex[b];
997  } else
998  originToSegment(current, a, b, A, B, AB, d, next, ray);
999  return false;
1000 }
1001 
1002 bool GJK::projectTriangleOrigin(const Simplex& current, Simplex& next) {
1003  const vertex_id_t a = 2, b = 1, c = 0;
1004  // A is the last point we added.
1005  const Vec3f &A = current.vertex[a]->w, B = current.vertex[b]->w,
1006  C = current.vertex[c]->w;
1007 
1008  const Vec3f AB = B - A, AC = C - A, ABC = AB.cross(AC);
1009 
1010  FCL_REAL edgeAC2o = ABC.cross(AC).dot(-A);
1011  if (edgeAC2o >= 0) {
1012  FCL_REAL towardsC = AC.dot(-A);
1013  if (towardsC >= 0) { // Region 1
1014  originToSegment(current, a, c, A, C, AC, towardsC, next, ray);
1015  free_v[nfree++] = current.vertex[b];
1016  } else { // Region 4 or 5
1017  FCL_REAL towardsB = AB.dot(-A);
1018  if (towardsB < 0) { // Region 5
1019  // A is the closest to the origin
1020  originToPoint(current, a, A, next, ray);
1021  free_v[nfree++] = current.vertex[b];
1022  } else // Region 4
1023  originToSegment(current, a, b, A, B, AB, towardsB, next, ray);
1024  free_v[nfree++] = current.vertex[c];
1025  }
1026  } else {
1027  FCL_REAL edgeAB2o = AB.cross(ABC).dot(-A);
1028  if (edgeAB2o >= 0) { // Region 4 or 5
1029  FCL_REAL towardsB = AB.dot(-A);
1030  if (towardsB < 0) { // Region 5
1031  // A is the closest to the origin
1032  originToPoint(current, a, A, next, ray);
1033  free_v[nfree++] = current.vertex[b];
1034  } else // Region 4
1035  originToSegment(current, a, b, A, B, AB, towardsB, next, ray);
1036  free_v[nfree++] = current.vertex[c];
1037  } else {
1038  return originToTriangle(current, a, b, c, ABC, ABC.dot(-A), next, ray);
1039  }
1040  }
1041  return false;
1042 }
1043 
1044 bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) {
1045  // The code of this function was generated using doc/gjk.py
1046  const vertex_id_t a = 3, b = 2, c = 1, d = 0;
1047  const Vec3f& A(current.vertex[a]->w);
1048  const Vec3f& B(current.vertex[b]->w);
1049  const Vec3f& C(current.vertex[c]->w);
1050  const Vec3f& D(current.vertex[d]->w);
1051  const FCL_REAL aa = A.squaredNorm();
1052  const FCL_REAL da = D.dot(A);
1053  const FCL_REAL db = D.dot(B);
1054  const FCL_REAL dc = D.dot(C);
1055  const FCL_REAL dd = D.dot(D);
1056  const FCL_REAL da_aa = da - aa;
1057  const FCL_REAL ca = C.dot(A);
1058  const FCL_REAL cb = C.dot(B);
1059  const FCL_REAL cc = C.dot(C);
1060  const FCL_REAL& cd = dc;
1061  const FCL_REAL ca_aa = ca - aa;
1062  const FCL_REAL ba = B.dot(A);
1063  const FCL_REAL bb = B.dot(B);
1064  const FCL_REAL& bc = cb;
1065  const FCL_REAL& bd = db;
1066  const FCL_REAL ba_aa = ba - aa;
1067  const FCL_REAL ba_ca = ba - ca;
1068  const FCL_REAL ca_da = ca - da;
1069  const FCL_REAL da_ba = da - ba;
1070  const Vec3f a_cross_b = A.cross(B);
1071  const Vec3f a_cross_c = A.cross(C);
1072 
1073  const FCL_REAL dummy_precision = Eigen::NumTraits<FCL_REAL>::epsilon();
1074  HPP_FCL_UNUSED_VARIABLE(dummy_precision);
1075 
1076 #define REGION_INSIDE() \
1077  ray.setZero(); \
1078  next.vertex[0] = current.vertex[d]; \
1079  next.vertex[1] = current.vertex[c]; \
1080  next.vertex[2] = current.vertex[b]; \
1081  next.vertex[3] = current.vertex[a]; \
1082  next.rank = 4; \
1083  return true;
1084 
1085  if (ba_aa <= 0) { // if AB.AO >= 0 / a10
1086  if (-D.dot(a_cross_b) <= 0) { // if ADB.AO >= 0 / a10.a3
1087  if (ba * da_ba + bd * ba_aa - bb * da_aa <=
1088  0) { // if (ADB ^ AB).AO >= 0 / a10.a3.a9
1089  if (da_aa <= 0) { // if AD.AO >= 0 / a10.a3.a9.a12
1090  assert(da * da_ba + dd * ba_aa - db * da_aa <=
1091  dummy_precision); // (ADB ^ AD).AO >= 0 / a10.a3.a9.a12.a8
1092  if (ba * ba_ca + bb * ca_aa - bc * ba_aa <=
1093  0) { // if (ABC ^ AB).AO >= 0 / a10.a3.a9.a12.a8.a4
1094  // Region ABC
1095  originToTriangle(current, a, b, c, (B - A).cross(C - A),
1096  -C.dot(a_cross_b), next, ray);
1097  free_v[nfree++] = current.vertex[d];
1098  } else { // not (ABC ^ AB).AO >= 0 / a10.a3.a9.a12.a8.!a4
1099  // Region AB
1100  originToSegment(current, a, b, A, B, B - A, -ba_aa, next, ray);
1101  free_v[nfree++] = current.vertex[c];
1102  free_v[nfree++] = current.vertex[d];
1103  } // end of (ABC ^ AB).AO >= 0
1104  } else { // not AD.AO >= 0 / a10.a3.a9.!a12
1105  if (ba * ba_ca + bb * ca_aa - bc * ba_aa <=
1106  0) { // if (ABC ^ AB).AO >= 0 / a10.a3.a9.!a12.a4
1107  if (ca * ba_ca + cb * ca_aa - cc * ba_aa <=
1108  0) { // if (ABC ^ AC).AO >= 0 / a10.a3.a9.!a12.a4.a5
1109  if (ca * ca_da + cc * da_aa - cd * ca_aa <=
1110  0) { // if (ACD ^ AC).AO >= 0 / a10.a3.a9.!a12.a4.a5.a6
1111  // Region ACD
1112  originToTriangle(current, a, c, d, (C - A).cross(D - A),
1113  -D.dot(a_cross_c), next, ray);
1114  free_v[nfree++] = current.vertex[b];
1115  } else { // not (ACD ^ AC).AO >= 0 / a10.a3.a9.!a12.a4.a5.!a6
1116  // Region AC
1117  originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray);
1118  free_v[nfree++] = current.vertex[b];
1119  free_v[nfree++] = current.vertex[d];
1120  } // end of (ACD ^ AC).AO >= 0
1121  } else { // not (ABC ^ AC).AO >= 0 / a10.a3.a9.!a12.a4.!a5
1122  // Region ABC
1123  originToTriangle(current, a, b, c, (B - A).cross(C - A),
1124  -C.dot(a_cross_b), next, ray);
1125  free_v[nfree++] = current.vertex[d];
1126  } // end of (ABC ^ AC).AO >= 0
1127  } else { // not (ABC ^ AB).AO >= 0 / a10.a3.a9.!a12.!a4
1128  // Region AB
1129  originToSegment(current, a, b, A, B, B - A, -ba_aa, next, ray);
1130  free_v[nfree++] = current.vertex[c];
1131  free_v[nfree++] = current.vertex[d];
1132  } // end of (ABC ^ AB).AO >= 0
1133  } // end of AD.AO >= 0
1134  } else { // not (ADB ^ AB).AO >= 0 / a10.a3.!a9
1135  if (da * da_ba + dd * ba_aa - db * da_aa <=
1136  0) { // if (ADB ^ AD).AO >= 0 / a10.a3.!a9.a8
1137  // Region ADB
1138  originToTriangle(current, a, d, b, (D - A).cross(B - A),
1139  D.dot(a_cross_b), next, ray);
1140  free_v[nfree++] = current.vertex[c];
1141  } else { // not (ADB ^ AD).AO >= 0 / a10.a3.!a9.!a8
1142  if (ca * ca_da + cc * da_aa - cd * ca_aa <=
1143  0) { // if (ACD ^ AC).AO >= 0 / a10.a3.!a9.!a8.a6
1144  if (da * ca_da + dc * da_aa - dd * ca_aa <=
1145  0) { // if (ACD ^ AD).AO >= 0 / a10.a3.!a9.!a8.a6.a7
1146  // Region AD
1147  originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray);
1148  free_v[nfree++] = current.vertex[b];
1149  free_v[nfree++] = current.vertex[c];
1150  } else { // not (ACD ^ AD).AO >= 0 / a10.a3.!a9.!a8.a6.!a7
1151  // Region ACD
1152  originToTriangle(current, a, c, d, (C - A).cross(D - A),
1153  -D.dot(a_cross_c), next, ray);
1154  free_v[nfree++] = current.vertex[b];
1155  } // end of (ACD ^ AD).AO >= 0
1156  } else { // not (ACD ^ AC).AO >= 0 / a10.a3.!a9.!a8.!a6
1157  if (da * ca_da + dc * da_aa - dd * ca_aa <=
1158  0) { // if (ACD ^ AD).AO >= 0 / a10.a3.!a9.!a8.!a6.a7
1159  // Region AD
1160  originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray);
1161  free_v[nfree++] = current.vertex[b];
1162  free_v[nfree++] = current.vertex[c];
1163  } else { // not (ACD ^ AD).AO >= 0 / a10.a3.!a9.!a8.!a6.!a7
1164  // Region AC
1165  originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray);
1166  free_v[nfree++] = current.vertex[b];
1167  free_v[nfree++] = current.vertex[d];
1168  } // end of (ACD ^ AD).AO >= 0
1169  } // end of (ACD ^ AC).AO >= 0
1170  } // end of (ADB ^ AD).AO >= 0
1171  } // end of (ADB ^ AB).AO >= 0
1172  } else { // not ADB.AO >= 0 / a10.!a3
1173  if (C.dot(a_cross_b) <= 0) { // if ABC.AO >= 0 / a10.!a3.a1
1174  if (ba * ba_ca + bb * ca_aa - bc * ba_aa <=
1175  0) { // if (ABC ^ AB).AO >= 0 / a10.!a3.a1.a4
1176  if (ca * ba_ca + cb * ca_aa - cc * ba_aa <=
1177  0) { // if (ABC ^ AC).AO >= 0 / a10.!a3.a1.a4.a5
1178  if (ca * ca_da + cc * da_aa - cd * ca_aa <=
1179  0) { // if (ACD ^ AC).AO >= 0 / a10.!a3.a1.a4.a5.a6
1180  // Region ACD
1181  originToTriangle(current, a, c, d, (C - A).cross(D - A),
1182  -D.dot(a_cross_c), next, ray);
1183  free_v[nfree++] = current.vertex[b];
1184  } else { // not (ACD ^ AC).AO >= 0 / a10.!a3.a1.a4.a5.!a6
1185  // Region AC
1186  originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray);
1187  free_v[nfree++] = current.vertex[b];
1188  free_v[nfree++] = current.vertex[d];
1189  } // end of (ACD ^ AC).AO >= 0
1190  } else { // not (ABC ^ AC).AO >= 0 / a10.!a3.a1.a4.!a5
1191  // Region ABC
1192  originToTriangle(current, a, b, c, (B - A).cross(C - A),
1193  -C.dot(a_cross_b), next, ray);
1194  free_v[nfree++] = current.vertex[d];
1195  } // end of (ABC ^ AC).AO >= 0
1196  } else { // not (ABC ^ AB).AO >= 0 / a10.!a3.a1.!a4
1197  // Region AB
1198  originToSegment(current, a, b, A, B, B - A, -ba_aa, next, ray);
1199  free_v[nfree++] = current.vertex[c];
1200  free_v[nfree++] = current.vertex[d];
1201  } // end of (ABC ^ AB).AO >= 0
1202  } else { // not ABC.AO >= 0 / a10.!a3.!a1
1203  if (D.dot(a_cross_c) <= 0) { // if ACD.AO >= 0 / a10.!a3.!a1.a2
1204  if (ca * ca_da + cc * da_aa - cd * ca_aa <=
1205  0) { // if (ACD ^ AC).AO >= 0 / a10.!a3.!a1.a2.a6
1206  if (da * ca_da + dc * da_aa - dd * ca_aa <=
1207  0) { // if (ACD ^ AD).AO >= 0 / a10.!a3.!a1.a2.a6.a7
1208  // Region AD
1209  originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray);
1210  free_v[nfree++] = current.vertex[b];
1211  free_v[nfree++] = current.vertex[c];
1212  } else { // not (ACD ^ AD).AO >= 0 / a10.!a3.!a1.a2.a6.!a7
1213  // Region ACD
1214  originToTriangle(current, a, c, d, (C - A).cross(D - A),
1215  -D.dot(a_cross_c), next, ray);
1216  free_v[nfree++] = current.vertex[b];
1217  } // end of (ACD ^ AD).AO >= 0
1218  } else { // not (ACD ^ AC).AO >= 0 / a10.!a3.!a1.a2.!a6
1219  if (ca_aa <= 0) { // if AC.AO >= 0 / a10.!a3.!a1.a2.!a6.a11
1220  // Region AC
1221  originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray);
1222  free_v[nfree++] = current.vertex[b];
1223  free_v[nfree++] = current.vertex[d];
1224  } else { // not AC.AO >= 0 / a10.!a3.!a1.a2.!a6.!a11
1225  // Region AD
1226  originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray);
1227  free_v[nfree++] = current.vertex[b];
1228  free_v[nfree++] = current.vertex[c];
1229  } // end of AC.AO >= 0
1230  } // end of (ACD ^ AC).AO >= 0
1231  } else { // not ACD.AO >= 0 / a10.!a3.!a1.!a2
1232  // Region Inside
1233  REGION_INSIDE()
1234  } // end of ACD.AO >= 0
1235  } // end of ABC.AO >= 0
1236  } // end of ADB.AO >= 0
1237  } else { // not AB.AO >= 0 / !a10
1238  if (ca_aa <= 0) { // if AC.AO >= 0 / !a10.a11
1239  if (D.dot(a_cross_c) <= 0) { // if ACD.AO >= 0 / !a10.a11.a2
1240  if (da_aa <= 0) { // if AD.AO >= 0 / !a10.a11.a2.a12
1241  if (ca * ca_da + cc * da_aa - cd * ca_aa <=
1242  0) { // if (ACD ^ AC).AO >= 0 / !a10.a11.a2.a12.a6
1243  if (da * ca_da + dc * da_aa - dd * ca_aa <=
1244  0) { // if (ACD ^ AD).AO >= 0 / !a10.a11.a2.a12.a6.a7
1245  if (da * da_ba + dd * ba_aa - db * da_aa <=
1246  0) { // if (ADB ^ AD).AO >= 0 / !a10.a11.a2.a12.a6.a7.a8
1247  // Region ADB
1248  originToTriangle(current, a, d, b, (D - A).cross(B - A),
1249  D.dot(a_cross_b), next, ray);
1250  free_v[nfree++] = current.vertex[c];
1251  } else { // not (ADB ^ AD).AO >= 0 / !a10.a11.a2.a12.a6.a7.!a8
1252  // Region AD
1253  originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray);
1254  free_v[nfree++] = current.vertex[b];
1255  free_v[nfree++] = current.vertex[c];
1256  } // end of (ADB ^ AD).AO >= 0
1257  } else { // not (ACD ^ AD).AO >= 0 / !a10.a11.a2.a12.a6.!a7
1258  // Region ACD
1259  originToTriangle(current, a, c, d, (C - A).cross(D - A),
1260  -D.dot(a_cross_c), next, ray);
1261  free_v[nfree++] = current.vertex[b];
1262  } // end of (ACD ^ AD).AO >= 0
1263  } else { // not (ACD ^ AC).AO >= 0 / !a10.a11.a2.a12.!a6
1264  assert(!(da * ca_da + dc * da_aa - dd * ca_aa <=
1265  dummy_precision)); // Not (ACD ^ AD).AO >= 0 /
1266  // !a10.a11.a2.a12.!a6.!a7
1267  if (ca * ba_ca + cb * ca_aa - cc * ba_aa <=
1268  0) { // if (ABC ^ AC).AO >= 0 / !a10.a11.a2.a12.!a6.!a7.a5
1269  // Region AC
1270  originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray);
1271  free_v[nfree++] = current.vertex[b];
1272  free_v[nfree++] = current.vertex[d];
1273  } else { // not (ABC ^ AC).AO >= 0 / !a10.a11.a2.a12.!a6.!a7.!a5
1274  // Region ABC
1275  originToTriangle(current, a, b, c, (B - A).cross(C - A),
1276  -C.dot(a_cross_b), next, ray);
1277  free_v[nfree++] = current.vertex[d];
1278  } // end of (ABC ^ AC).AO >= 0
1279  } // end of (ACD ^ AC).AO >= 0
1280  } else { // not AD.AO >= 0 / !a10.a11.a2.!a12
1281  if (ca * ba_ca + cb * ca_aa - cc * ba_aa <=
1282  0) { // if (ABC ^ AC).AO >= 0 / !a10.a11.a2.!a12.a5
1283  if (ca * ca_da + cc * da_aa - cd * ca_aa <=
1284  0) { // if (ACD ^ AC).AO >= 0 / !a10.a11.a2.!a12.a5.a6
1285  assert(!(da * ca_da + dc * da_aa - dd * ca_aa <=
1286  -dummy_precision)); // Not (ACD ^ AD).AO >= 0 /
1287  // !a10.a11.a2.!a12.a5.a6.!a7
1288  // Region ACD
1289  originToTriangle(current, a, c, d, (C - A).cross(D - A),
1290  -D.dot(a_cross_c), next, ray);
1291  free_v[nfree++] = current.vertex[b];
1292  } else { // not (ACD ^ AC).AO >= 0 / !a10.a11.a2.!a12.a5.!a6
1293  // Region AC
1294  originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray);
1295  free_v[nfree++] = current.vertex[b];
1296  free_v[nfree++] = current.vertex[d];
1297  } // end of (ACD ^ AC).AO >= 0
1298  } else { // not (ABC ^ AC).AO >= 0 / !a10.a11.a2.!a12.!a5
1299  if (C.dot(a_cross_b) <=
1300  0) { // if ABC.AO >= 0 / !a10.a11.a2.!a12.!a5.a1
1301  assert(ba * ba_ca + bb * ca_aa - bc * ba_aa <=
1302  dummy_precision); // (ABC ^ AB).AO >= 0 /
1303  // !a10.a11.a2.!a12.!a5.a1.a4
1304  // Region ABC
1305  originToTriangle(current, a, b, c, (B - A).cross(C - A),
1306  -C.dot(a_cross_b), next, ray);
1307  free_v[nfree++] = current.vertex[d];
1308  } else { // not ABC.AO >= 0 / !a10.a11.a2.!a12.!a5.!a1
1309  assert(!(da * ca_da + dc * da_aa - dd * ca_aa <=
1310  -dummy_precision)); // Not (ACD ^ AD).AO >= 0 /
1311  // !a10.a11.a2.!a12.!a5.!a1.!a7
1312  // Region ACD
1313  originToTriangle(current, a, c, d, (C - A).cross(D - A),
1314  -D.dot(a_cross_c), next, ray);
1315  free_v[nfree++] = current.vertex[b];
1316  } // end of ABC.AO >= 0
1317  } // end of (ABC ^ AC).AO >= 0
1318  } // end of AD.AO >= 0
1319  } else { // not ACD.AO >= 0 / !a10.a11.!a2
1320  if (C.dot(a_cross_b) <= 0) { // if ABC.AO >= 0 / !a10.a11.!a2.a1
1321  if (ca * ba_ca + cb * ca_aa - cc * ba_aa <=
1322  0) { // if (ABC ^ AC).AO >= 0 / !a10.a11.!a2.a1.a5
1323  // Region AC
1324  originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray);
1325  free_v[nfree++] = current.vertex[b];
1326  free_v[nfree++] = current.vertex[d];
1327  } else { // not (ABC ^ AC).AO >= 0 / !a10.a11.!a2.a1.!a5
1328  assert(ba * ba_ca + bb * ca_aa - bc * ba_aa <=
1329  dummy_precision); // (ABC ^ AB).AO >= 0 /
1330  // !a10.a11.!a2.a1.!a5.a4
1331  // Region ABC
1332  originToTriangle(current, a, b, c, (B - A).cross(C - A),
1333  -C.dot(a_cross_b), next, ray);
1334  free_v[nfree++] = current.vertex[d];
1335  } // end of (ABC ^ AC).AO >= 0
1336  } else { // not ABC.AO >= 0 / !a10.a11.!a2.!a1
1337  if (-D.dot(a_cross_b) <= 0) { // if ADB.AO >= 0 / !a10.a11.!a2.!a1.a3
1338  if (da * da_ba + dd * ba_aa - db * da_aa <=
1339  0) { // if (ADB ^ AD).AO >= 0 / !a10.a11.!a2.!a1.a3.a8
1340  // Region ADB
1341  originToTriangle(current, a, d, b, (D - A).cross(B - A),
1342  D.dot(a_cross_b), next, ray);
1343  free_v[nfree++] = current.vertex[c];
1344  } else { // not (ADB ^ AD).AO >= 0 / !a10.a11.!a2.!a1.a3.!a8
1345  // Region AD
1346  originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray);
1347  free_v[nfree++] = current.vertex[b];
1348  free_v[nfree++] = current.vertex[c];
1349  } // end of (ADB ^ AD).AO >= 0
1350  } else { // not ADB.AO >= 0 / !a10.a11.!a2.!a1.!a3
1351  // Region Inside
1352  REGION_INSIDE()
1353  } // end of ADB.AO >= 0
1354  } // end of ABC.AO >= 0
1355  } // end of ACD.AO >= 0
1356  } else { // not AC.AO >= 0 / !a10.!a11
1357  if (da_aa <= 0) { // if AD.AO >= 0 / !a10.!a11.a12
1358  if (-D.dot(a_cross_b) <= 0) { // if ADB.AO >= 0 / !a10.!a11.a12.a3
1359  if (da * ca_da + dc * da_aa - dd * ca_aa <=
1360  0) { // if (ACD ^ AD).AO >= 0 / !a10.!a11.a12.a3.a7
1361  if (da * da_ba + dd * ba_aa - db * da_aa <=
1362  0) { // if (ADB ^ AD).AO >= 0 / !a10.!a11.a12.a3.a7.a8
1363  assert(!(ba * da_ba + bd * ba_aa - bb * da_aa <=
1364  -dummy_precision)); // Not (ADB ^ AB).AO >= 0 /
1365  // !a10.!a11.a12.a3.a7.a8.!a9
1366  // Region ADB
1367  originToTriangle(current, a, d, b, (D - A).cross(B - A),
1368  D.dot(a_cross_b), next, ray);
1369  free_v[nfree++] = current.vertex[c];
1370  } else { // not (ADB ^ AD).AO >= 0 / !a10.!a11.a12.a3.a7.!a8
1371  // Region AD
1372  originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray);
1373  free_v[nfree++] = current.vertex[b];
1374  free_v[nfree++] = current.vertex[c];
1375  } // end of (ADB ^ AD).AO >= 0
1376  } else { // not (ACD ^ AD).AO >= 0 / !a10.!a11.a12.a3.!a7
1377  if (D.dot(a_cross_c) <=
1378  0) { // if ACD.AO >= 0 / !a10.!a11.a12.a3.!a7.a2
1379  assert(ca * ca_da + cc * da_aa - cd * ca_aa <=
1380  dummy_precision); // (ACD ^ AC).AO >= 0 /
1381  // !a10.!a11.a12.a3.!a7.a2.a6
1382  // Region ACD
1383  originToTriangle(current, a, c, d, (C - A).cross(D - A),
1384  -D.dot(a_cross_c), next, ray);
1385  free_v[nfree++] = current.vertex[b];
1386  } else { // not ACD.AO >= 0 / !a10.!a11.a12.a3.!a7.!a2
1387  if (C.dot(a_cross_b) <=
1388  0) { // if ABC.AO >= 0 / !a10.!a11.a12.a3.!a7.!a2.a1
1389  assert(!(ba * ba_ca + bb * ca_aa - bc * ba_aa <=
1390  -dummy_precision)); // Not (ABC ^ AB).AO >= 0 /
1391  // !a10.!a11.a12.a3.!a7.!a2.a1.!a4
1392  // Region ADB
1393  originToTriangle(current, a, d, b, (D - A).cross(B - A),
1394  D.dot(a_cross_b), next, ray);
1395  free_v[nfree++] = current.vertex[c];
1396  } else { // not ABC.AO >= 0 / !a10.!a11.a12.a3.!a7.!a2.!a1
1397  // Region ADB
1398  originToTriangle(current, a, d, b, (D - A).cross(B - A),
1399  D.dot(a_cross_b), next, ray);
1400  free_v[nfree++] = current.vertex[c];
1401  } // end of ABC.AO >= 0
1402  } // end of ACD.AO >= 0
1403  } // end of (ACD ^ AD).AO >= 0
1404  } else { // not ADB.AO >= 0 / !a10.!a11.a12.!a3
1405  if (D.dot(a_cross_c) <= 0) { // if ACD.AO >= 0 / !a10.!a11.a12.!a3.a2
1406  if (da * ca_da + dc * da_aa - dd * ca_aa <=
1407  0) { // if (ACD ^ AD).AO >= 0 / !a10.!a11.a12.!a3.a2.a7
1408  // Region AD
1409  originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray);
1410  free_v[nfree++] = current.vertex[b];
1411  free_v[nfree++] = current.vertex[c];
1412  } else { // not (ACD ^ AD).AO >= 0 / !a10.!a11.a12.!a3.a2.!a7
1413  assert(ca * ca_da + cc * da_aa - cd * ca_aa <=
1414  dummy_precision); // (ACD ^ AC).AO >= 0 /
1415  // !a10.!a11.a12.!a3.a2.!a7.a6
1416  // Region ACD
1417  originToTriangle(current, a, c, d, (C - A).cross(D - A),
1418  -D.dot(a_cross_c), next, ray);
1419  free_v[nfree++] = current.vertex[b];
1420  } // end of (ACD ^ AD).AO >= 0
1421  } else { // not ACD.AO >= 0 / !a10.!a11.a12.!a3.!a2
1422  // Region Inside
1423  REGION_INSIDE()
1424  } // end of ACD.AO >= 0
1425  } // end of ADB.AO >= 0
1426  } else { // not AD.AO >= 0 / !a10.!a11.!a12
1427  // Region A
1428  originToPoint(current, a, A, next, ray);
1429  free_v[nfree++] = current.vertex[b];
1430  free_v[nfree++] = current.vertex[c];
1431  free_v[nfree++] = current.vertex[d];
1432  } // end of AD.AO >= 0
1433  } // end of AC.AO >= 0
1434  } // end of AB.AO >= 0
1435 
1436 #undef REGION_INSIDE
1437  return false;
1438 }
1439 
1440 void EPA::initialize() {
1441  sv_store = new SimplexV[max_vertex_num];
1442  fc_store = new SimplexF[max_face_num];
1443  status = Failed;
1444  normal = Vec3f(0, 0, 0);
1445  depth = 0;
1446  nextsv = 0;
1447  for (size_t i = 0; i < max_face_num; ++i)
1448  stock.append(&fc_store[max_face_num - i - 1]);
1449 }
1450 
1451 bool EPA::getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b,
1452  FCL_REAL& dist) {
1453  Vec3f ab = b->w - a->w;
1454  Vec3f n_ab = ab.cross(face->n);
1455  FCL_REAL a_dot_nab = a->w.dot(n_ab);
1456 
1457  if (a_dot_nab < 0) // the origin is on the outside part of ab
1458  {
1459  // following is similar to projectOrigin for two points
1460  // however, as we dont need to compute the parameterization, dont need to
1461  // compute 0 or 1
1462  FCL_REAL a_dot_ab = a->w.dot(ab);
1463  FCL_REAL b_dot_ab = b->w.dot(ab);
1464 
1465  if (a_dot_ab > 0)
1466  dist = a->w.norm();
1467  else if (b_dot_ab < 0)
1468  dist = b->w.norm();
1469  else {
1470  dist = std::sqrt(std::max(
1471  a->w.squaredNorm() - a_dot_ab * a_dot_ab / ab.squaredNorm(), 0.));
1472  }
1473 
1474  return true;
1475  }
1476 
1477  return false;
1478 }
1479 
1480 EPA::SimplexF* EPA::newFace(SimplexV* a, SimplexV* b, SimplexV* c,
1481  bool forced) {
1482  if (stock.root) {
1483  SimplexF* face = stock.root;
1484  stock.remove(face);
1485  hull.append(face);
1486  face->pass = 0;
1487  face->vertex[0] = a;
1488  face->vertex[1] = b;
1489  face->vertex[2] = c;
1490  face->n = (b->w - a->w).cross(c->w - a->w);
1491  FCL_REAL l = face->n.norm();
1492 
1493  if (l > Eigen::NumTraits<FCL_REAL>::epsilon()) {
1494  face->n /= l;
1495 
1496  if (!(getEdgeDist(face, a, b, face->d) ||
1497  getEdgeDist(face, b, c, face->d) ||
1498  getEdgeDist(face, c, a, face->d))) {
1499  face->d = a->w.dot(face->n);
1500  }
1501 
1502  if (forced || face->d >= -tolerance)
1503  return face;
1504  else
1505  status = NonConvex;
1506  } else
1507  status = Degenerated;
1508 
1509  hull.remove(face);
1510  stock.append(face);
1511  return NULL;
1512  }
1513 
1514  status = stock.root ? OutOfVertices : OutOfFaces;
1515  return NULL;
1516 }
1517 
1519 EPA::SimplexF* EPA::findBest() {
1520  SimplexF* minf = hull.root;
1521  FCL_REAL mind = minf->d * minf->d;
1522  for (SimplexF* f = minf->l[1]; f; f = f->l[1]) {
1523  FCL_REAL sqd = f->d * f->d;
1524  if (sqd < mind) {
1525  minf = f;
1526  mind = sqd;
1527  }
1528  }
1529  return minf;
1530 }
1531 
1532 EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) {
1533  GJK::Simplex& simplex = *gjk.getSimplex();
1534  support_func_guess_t hint(gjk.support_hint);
1535  if ((simplex.rank > 1) && gjk.encloseOrigin()) {
1536  while (hull.root) {
1537  SimplexF* f = hull.root;
1538  hull.remove(f);
1539  stock.append(f);
1540  }
1541 
1542  status = Valid;
1543  nextsv = 0;
1544 
1545  if ((simplex.vertex[0]->w - simplex.vertex[3]->w)
1546  .dot((simplex.vertex[1]->w - simplex.vertex[3]->w)
1547  .cross(simplex.vertex[2]->w - simplex.vertex[3]->w)) < 0) {
1548  SimplexV* tmp = simplex.vertex[0];
1549  simplex.vertex[0] = simplex.vertex[1];
1550  simplex.vertex[1] = tmp;
1551  }
1552 
1553  SimplexF* tetrahedron[] = {
1554  newFace(simplex.vertex[0], simplex.vertex[1], simplex.vertex[2], true),
1555  newFace(simplex.vertex[1], simplex.vertex[0], simplex.vertex[3], true),
1556  newFace(simplex.vertex[2], simplex.vertex[1], simplex.vertex[3], true),
1557  newFace(simplex.vertex[0], simplex.vertex[2], simplex.vertex[3], true)};
1558 
1559  if (hull.count == 4) {
1560  SimplexF* best = findBest(); // find the best face (the face with the
1561  // minimum distance to origin) to split
1562  SimplexF outer = *best;
1563  size_t pass = 0;
1564  size_t iterations = 0;
1565 
1566  // set the face connectivity
1567  bind(tetrahedron[0], 0, tetrahedron[1], 0);
1568  bind(tetrahedron[0], 1, tetrahedron[2], 0);
1569  bind(tetrahedron[0], 2, tetrahedron[3], 0);
1570  bind(tetrahedron[1], 1, tetrahedron[3], 2);
1571  bind(tetrahedron[1], 2, tetrahedron[2], 1);
1572  bind(tetrahedron[2], 2, tetrahedron[3], 1);
1573 
1574  status = Valid;
1575  for (; iterations < max_iterations; ++iterations) {
1576  if (nextsv >= max_vertex_num) {
1577  status = OutOfVertices;
1578  break;
1579  }
1580 
1581  SimplexHorizon horizon;
1582  SimplexV* w = &sv_store[nextsv++];
1583  bool valid = true;
1584  best->pass = ++pass;
1585  // At the moment, SimplexF.n is always normalized. This could be revised
1586  // in the future...
1587  gjk.getSupport(best->n, true, *w, hint);
1588  FCL_REAL wdist = best->n.dot(w->w) - best->d;
1589  if (wdist <= tolerance) {
1590  status = AccuracyReached;
1591  break;
1592  }
1593  for (size_t j = 0; (j < 3) && valid; ++j)
1594  valid &= expand(pass, w, best->f[j], best->e[j], horizon);
1595 
1596  if (!valid || horizon.nf < 3) {
1597  // The status has already been set by the expand function.
1598  assert(!(status & Valid));
1599  break;
1600  }
1601  // need to add the edge connectivity between first and last faces
1602  bind(horizon.ff, 2, horizon.cf, 1);
1603  hull.remove(best);
1604  stock.append(best);
1605  best = findBest();
1606  outer = *best;
1607  }
1608 
1609  normal = outer.n;
1610  depth = outer.d;
1611  result.rank = 3;
1612  result.vertex[0] = outer.vertex[0];
1613  result.vertex[1] = outer.vertex[1];
1614  result.vertex[2] = outer.vertex[2];
1615  return status;
1616  }
1617  }
1618 
1619  // FallBack when the simplex given by GJK is of rank 1.
1620  // Since the simplex only contains support points which convex
1621  // combination describe the origin, the point in the simplex is actually
1622  // the origin.
1623  status = FallBack;
1624  // TODO: define a better normal
1625  assert(simplex.rank == 1 && simplex.vertex[0]->w.isZero(gjk.getTolerance()));
1626  normal = -guess;
1627  FCL_REAL nl = normal.norm();
1628  if (nl > 0)
1629  normal /= nl;
1630  else
1631  normal = Vec3f(1, 0, 0);
1632  depth = 0;
1633  result.rank = 1;
1634  result.vertex[0] = simplex.vertex[0];
1635  return status;
1636 }
1637 
1639 bool EPA::expand(size_t pass, SimplexV* w, SimplexF* f, size_t e,
1640  SimplexHorizon& horizon) {
1641  static const size_t nexti[] = {1, 2, 0};
1642  static const size_t previ[] = {2, 0, 1};
1643 
1644  if (f->pass == pass) {
1645  status = InvalidHull;
1646  return false;
1647  }
1648 
1649  const size_t e1 = nexti[e];
1650 
1651  // case 1: the new face is not degenerated, i.e., the new face is not coplanar
1652  // with the old face f.
1653  if (f->n.dot(w->w - f->vertex[e]->w) <
1654  -Eigen::NumTraits<FCL_REAL>::epsilon()) {
1655  SimplexF* nf = newFace(f->vertex[e1], f->vertex[e], w, false);
1656  if (nf) {
1657  // add face-face connectivity
1658  bind(nf, 0, f, e);
1659 
1660  // if there is last face in the horizon, then need to add another
1661  // connectivity, i.e. the edge connecting the current new add edge and the
1662  // last new add edge. This does not finish all the connectivities because
1663  // the final face need to connect with the first face, this will be
1664  // handled in the evaluate function. Notice the face is anti-clockwise, so
1665  // the edges are 0 (bottom), 1 (right), 2 (left)
1666  if (horizon.cf)
1667  bind(nf, 2, horizon.cf, 1);
1668  else
1669  horizon.ff = nf;
1670 
1671  horizon.cf = nf;
1672  ++horizon.nf;
1673  return true;
1674  }
1675  return false;
1676  }
1677 
1678  // case 2: the new face is coplanar with the old face f. We need to add two
1679  // faces and delete the old face
1680  const size_t e2 = previ[e];
1681  f->pass = pass;
1682  if (expand(pass, w, f->f[e1], f->e[e1], horizon) &&
1683  expand(pass, w, f->f[e2], f->e[e2], horizon)) {
1684  hull.remove(f);
1685  stock.append(f);
1686  return true;
1687  }
1688  return false;
1689 }
1690 
1691 bool EPA::getClosestPoints(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) {
1692  bool res = details::getClosestPoints(result, w0, w1);
1693  if (!res) return false;
1694  details::inflate<false>(shape, w0, w1);
1695  return true;
1696 }
1697 
1698 } // namespace details
1699 
1700 } // namespace fcl
1701 
1702 } // namespace hpp
hpp::fcl::GEOM_BOX
Definition: collision_object.h:75
hpp::fcl::details::EPA::findBest
SimplexF * findBest()
Find the best polytope face to split.
Definition: src/narrowphase/gjk.cpp:1519
hpp::fcl::Box::halfSide
Vec3f halfSide
box side half-length
Definition: shape/geometric_shapes.h:148
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
hpp::fcl::details::EPA::newFace
SimplexF * newFace(SimplexV *a, SimplexV *b, SimplexV *vertex, bool forced)
Definition: src/narrowphase/gjk.cpp:1480
hpp::fcl::details::GJK::Simplex::vertex
SimplexV * vertex[4]
simplex vertex
Definition: gjk.h:154
hpp::fcl::details::makeGetSupportFunction1
MinkowskiDiff::GetSupportFunction makeGetSupportFunction1(const ShapeBase *s1, bool identity, Eigen::Array< FCL_REAL, 1, 2 > &inflation, int linear_log_convex_threshold)
Definition: src/narrowphase/gjk.cpp:341
hpp::fcl::details::getSupportTpl
void getSupportTpl(const Shape0 *s0, const Shape1 *s1, const Matrix3f &oR1, const Vec3f &ot1, const Vec3f &dir, Vec3f &support0, Vec3f &support1, support_func_guess_t &hint, MinkowskiDiff::ShapeData data[2])
Definition: src/narrowphase/gjk.cpp:301
axis
axis
hpp::fcl::details::GJK::support_hint
support_func_guess_t support_hint
Definition: gjk.h:172
generate_distance_plot.b
float b
Definition: generate_distance_plot.py:8
hpp::fcl::details::getShapeSupportLinear
void getShapeSupportLinear(const ConvexBase *convex, const Vec3f &dir, Vec3f &support, int &hint, MinkowskiDiff::ShapeData *)
Definition: src/narrowphase/gjk.cpp:210
hpp::fcl::Ellipsoid
Ellipsoid centered at point zero.
Definition: shape/geometric_shapes.h:258
y
y
hpp::fcl::details::MinkowskiDiff::GetSupportFunction
void(* GetSupportFunction)(const MinkowskiDiff &minkowskiDiff, const Vec3f &dir, bool dirIsNormalized, Vec3f &support0, Vec3f &support1, support_func_guess_t &hint, ShapeData data[2])
Definition: gjk.h:97
hpp::fcl::details::getNormalizeSupportDirectionFromShapes
void getNormalizeSupportDirectionFromShapes(const ShapeBase *shape0, const ShapeBase *shape1, bool &normalize_support_direction)
Definition: src/narrowphase/gjk.cpp:481
hpp::fcl::GJKVariant
GJKVariant
Variant to use for the GJK algorithm.
Definition: data_types.h:83
hpp::fcl::TriangleP::b
Vec3f b
Definition: shape/geometric_shapes.h:109
hpp::fcl::Cone::halfLength
FCL_REAL halfLength
Half Length along z axis.
Definition: shape/geometric_shapes.h:433
hpp::fcl::GEOM_CONE
Definition: collision_object.h:78
hpp::fcl::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: shape/geometric_shapes.h:501
hpp::fcl::details::originToTriangle
bool originToTriangle(const GJK::Simplex &current, GJK::vertex_id_t a, GJK::vertex_id_t b, GJK::vertex_id_t c, const Vec3f &ABC, const FCL_REAL &ABCdotAO, GJK::Simplex &next, Vec3f &ray)
Definition: src/narrowphase/gjk.cpp:948
hpp::fcl::details::MinkowskiDiff::ShapeData::visited
std::vector< int8_t > visited
Definition: gjk.h:66
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::details::GJK::appendVertex
void appendVertex(Simplex &simplex, const Vec3f &v, bool isNormalized, support_func_guess_t &hint)
append one vertex to the simplex
Definition: src/narrowphase/gjk.cpp:862
hpp::fcl::details::GJK::encloseOrigin
bool encloseOrigin()
whether the simplex enclose the origin
Definition: src/narrowphase/gjk.cpp:868
epsilon
static FCL_REAL epsilon
Definition: simple.cpp:12
hpp::fcl::details::GJK::getTolerance
FCL_REAL getTolerance()
Get GJK tolerance.
Definition: gjk.h:258
CALL_GET_SHAPE_SUPPORT
#define CALL_GET_SHAPE_SUPPORT(ShapeType)
Definition: src/narrowphase/gjk.cpp:252
hpp::fcl::details::GJK::removeVertex
void removeVertex(Simplex &simplex)
discard one vertex from the simplex
Definition: src/narrowphase/gjk.cpp:858
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
hpp::fcl::details::getSupport
Vec3f getSupport(const ShapeBase *shape, const Vec3f &dir, bool dirIsNormalized, int &hint)
the support function for shape
Definition: src/narrowphase/gjk.cpp:260
hpp::fcl::details::getShapeSupportLog
void getShapeSupportLog(const ConvexBase *convex, const Vec3f &dir, Vec3f &support, int &hint, MinkowskiDiff::ShapeData *data)
Definition: src/narrowphase/gjk.cpp:169
hpp::fcl::details::getNormalizeSupportDirection
bool getNormalizeSupportDirection(const ShapeBase *shape)
Definition: src/narrowphase/gjk.cpp:450
hpp::fcl::details::EPA::getClosestPoints
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
Definition: src/narrowphase/gjk.cpp:1691
hpp::fcl::details::MinkowskiDiff::set
void set(const ShapeBase *shape0, const ShapeBase *shape1)
Definition: src/narrowphase/gjk.cpp:505
hpp::fcl::TriangleP::c
Vec3f c
Definition: shape/geometric_shapes.h:109
hpp::fcl::details::GJK
class for GJK algorithm
Definition: gjk.h:141
c
c
data
data
hpp::fcl::details::GJK::SimplexV::w
Vec3f w
support vector (i.e., the furthest point on the shape along the support direction) ...
Definition: gjk.h:147
hpp::fcl::details::MinkowskiDiff
Minkowski difference class of two shapes.
Definition: gjk.h:59
D
D
hpp::fcl::details::GJK::projectTetrahedraOrigin
bool projectTetrahedraOrigin(const Simplex &current, Simplex &next)
Project origin (0) onto tetrahedron a-b-c-d See projectLineOrigin for an explanation on simplex proje...
Definition: src/narrowphase/gjk.cpp:1044
hpp::fcl::details::getShapeSupport
void getShapeSupport(const TriangleP *triangle, const Vec3f &dir, Vec3f &support, int &, MinkowskiDiff::ShapeData *)
Definition: src/narrowphase/gjk.cpp:48
obb.v
list v
Definition: obb.py:45
hpp::fcl::GEOM_HALFSPACE
Definition: collision_object.h:82
hpp::fcl::ShapeBase
Base class for all basic geometric shapes.
Definition: shape/geometric_shapes.h:51
hpp::fcl::details::GJK::getClosestPoints
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
Definition: src/narrowphase/gjk.cpp:617
hpp::fcl::details::GJK::getGuessFromSimplex
Vec3f getGuessFromSimplex() const
get the guess from current simplex
Definition: src/narrowphase/gjk.cpp:528
hpp::fcl::CollisionGeometry::getNodeType
virtual NODE_TYPE getNodeType() const
get the node type
Definition: collision_object.h:131
hpp::fcl::ConvexBase::num_points
unsigned int num_points
Definition: shape/geometric_shapes.h:624
hpp::fcl::distance
HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS.cpp:181
hpp::fcl::Cone::radius
FCL_REAL radius
Radius of the cone.
Definition: shape/geometric_shapes.h:427
hpp::fcl::details::GJK::projectLineOrigin
bool projectLineOrigin(const Simplex &current, Simplex &next)
Project origin (0) onto line a-b For a detailed explanation of how to efficiently project onto a simp...
Definition: src/narrowphase/gjk.cpp:974
REGION_INSIDE
#define REGION_INSIDE()
HPP_FCL_UNUSED_VARIABLE
#define HPP_FCL_UNUSED_VARIABLE(var)
Definition: include/hpp/fcl/fwd.hh:55
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::details::MinkowskiDiff::ot1
Vec3f ot1
translation from shape1 to shape0 such that .
Definition: gjk.h:79
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: shape/geometric_shapes.h:125
hpp::fcl::GEOM_TRIANGLE
Definition: collision_object.h:83
hpp::fcl::details::GJK::projectTriangleOrigin
bool projectTriangleOrigin(const Simplex &current, Simplex &next)
Project origin (0) onto triangle a-b-c See projectLineOrigin for an explanation on simplex projection...
Definition: src/narrowphase/gjk.cpp:1002
hpp::fcl::details::GJK::checkConvergence
bool checkConvergence(const Vec3f &w, const FCL_REAL &rl, FCL_REAL &alpha, const FCL_REAL &omega)
Convergence check used to stop GJK when shapes are not in collision.
Definition: src/narrowphase/gjk.cpp:793
hpp::fcl::details::GJK::Status
Status
Status of the GJK algorithm: Valid: GJK converged and the shapes are not in collision. Inside: GJK converged and the shapes are in collision. Failed: GJK did not converge.
Definition: gjk.h:165
hpp::fcl::details::MinkowskiDiff::inflation
Array2d inflation
The radius of the sphere swepted volume. The 2 values correspond to the inflation of shape 0 and shap...
Definition: gjk.h:84
hpp::fcl::details::GJK::getSimplex
Simplex * getSimplex() const
get the underlying simplex using in GJK, can be used for cache in next iteration
Definition: gjk.h:221
hpp::fcl::TriangleP
Triangle stores the points instead of only indices of points.
Definition: shape/geometric_shapes.h:71
hpp::fcl::Transform3f::getTranslation
const Vec3f & getTranslation() const
get translation
Definition: transform.h:100
hpp::fcl::support_func_guess_t
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:72
hpp::fcl::details::originToPoint
void originToPoint(const GJK::Simplex &current, GJK::vertex_id_t a, const Vec3f &A, GJK::Simplex &next, Vec3f &ray)
Definition: src/narrowphase/gjk.cpp:925
hpp::fcl::Cone
Cone The base of the cone is at and the top is at .
Definition: shape/geometric_shapes.h:414
hpp::fcl::details::GJK::SimplexV::w0
Vec3f w0
support vector for shape 0 and 1.
Definition: gjk.h:144
hpp::fcl::TriangleP::a
Vec3f a
Definition: shape/geometric_shapes.h:109
hpp::fcl::details::GJK::vertex_id_t
unsigned char vertex_id_t
Definition: gjk.h:150
hpp::fcl::Cylinder::radius
FCL_REAL radius
Radius of the cylinder.
Definition: shape/geometric_shapes.h:522
hpp::fcl::details::makeGetSupportFunction0
MinkowskiDiff::GetSupportFunction makeGetSupportFunction0(const ShapeBase *s0, const ShapeBase *s1, bool identity, Eigen::Array< FCL_REAL, 1, 2 > &inflation, int linear_log_convex_threshold)
Definition: src/narrowphase/gjk.cpp:401
hpp::fcl::Sphere
Center at zero point sphere.
Definition: shape/geometric_shapes.h:196
hpp::fcl::Capsule
Capsule It is where is the distance between the point x and the capsule segment AB...
Definition: shape/geometric_shapes.h:333
hpp::fcl::GEOM_SPHERE
Definition: collision_object.h:76
hpp::fcl::detail::select
size_t select(const NodeBase< BV > &query, const NodeBase< BV > &node1, const NodeBase< BV > &node2)
select from node1 and node2 which is close to a given query. 0 for node1 and 1 for node2 ...
Definition: hierarchy_tree-inl.h:952
hpp::fcl::ConvexBase
Base for convex polytope.
Definition: shape/geometric_shapes.h:581
gjk
Definition: doc/gjk.py:1
hpp::fcl::GEOM_ELLIPSOID
Definition: collision_object.h:85
hpp::fcl::details::MinkowskiDiff::shapes
const ShapeBase * shapes[2]
points to two shapes
Definition: gjk.h:63
B
B
hpp::fcl::ConvexBase::Neighbors::count
unsigned char const & count() const
Definition: shape/geometric_shapes.h:630
res
res
hpp::fcl::GEOM_CYLINDER
Definition: collision_object.h:79
hpp::fcl::details::EPA::getEdgeDist
bool getEdgeDist(SimplexF *face, SimplexV *a, SimplexV *b, FCL_REAL &dist)
Definition: src/narrowphase/gjk.cpp:1451
geometric_shapes_traits.h
hpp::fcl::details::EPA::evaluate
Status evaluate(GJK &gjk, const Vec3f &guess)
Definition: src/narrowphase/gjk.cpp:1532
setZero
void setZero(std::vector< MatType, Eigen::aligned_allocator< MatType > > &Ms)
hpp::fcl::shape_traits
Definition: geometric_shapes_traits.h:55
hpp::fcl::details::details::inflate
void inflate(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
Inflate the points.
Definition: src/narrowphase/gjk.cpp:592
collision_manager.box
box
Definition: collision_manager.py:11
hpp::fcl::Cylinder::halfLength
FCL_REAL halfLength
Half Length along z axis.
Definition: shape/geometric_shapes.h:528
c2
c2
a
list a
hpp::fcl::details::GJK::Simplex::rank
vertex_id_t rank
size of simplex (number of vertices)
Definition: gjk.h:156
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::Ellipsoid::radii
Vec3f radii
Radii of the Ellipsoid (such that on boundary: x^2/rx^2 + y^2/ry^2.
Definition: shape/geometric_shapes.h:271
A
A
hpp::fcl::details::getSupportFuncTpl
void getSupportFuncTpl(const MinkowskiDiff &md, const Vec3f &dir, bool dirIsNormalized, Vec3f &support0, Vec3f &support1, support_func_guess_t &hint, MinkowskiDiff::ShapeData data[2])
Definition: src/narrowphase/gjk.cpp:315
hpp::fcl::details::MinkowskiDiff::oR1
Matrix3f oR1
rotation from shape1 to shape0 such that .
Definition: gjk.h:75
hpp::fcl::ConvexBase::neighbors
Neighbors * neighbors
Definition: shape/geometric_shapes.h:655
hpp::fcl::Transform3f::getRotation
const Matrix3f & getRotation() const
get rotation
Definition: transform.h:109
hpp::fcl::GEOM_PLANE
Definition: collision_object.h:81
hpp::fcl::details::originToSegment
void originToSegment(const GJK::Simplex &current, GJK::vertex_id_t a, GJK::vertex_id_t b, const Vec3f &A, const Vec3f &B, const Vec3f &AB, const FCL_REAL &ABdotAO, GJK::Simplex &next, Vec3f &ray)
Definition: src/narrowphase/gjk.cpp:933
hpp::fcl::GEOM_CONVEX
Definition: collision_object.h:80
hpp::fcl::details::details::getClosestPoints
bool getClosestPoints(const GJK::Simplex &simplex, Vec3f &w0, Vec3f &w1)
Definition: src/narrowphase/gjk.cpp:532
hpp::fcl::details::EPA::expand
bool expand(size_t pass, SimplexV *w, SimplexF *f, size_t e, SimplexHorizon &horizon)
the goal is to add a face connecting vertex w and face edge f[e]
Definition: src/narrowphase/gjk.cpp:1639
hpp::fcl::GEOM_CAPSULE
Definition: collision_object.h:77
hpp::fcl::details::GJK::getSupport
void getSupport(const Vec3f &d, bool dIsNormalized, SimplexV &sv, support_func_guess_t &hint) const
apply the support function along a direction, the result is return in sv
Definition: gjk.h:210
hpp::fcl::triple
static Derived::Scalar triple(const Eigen::MatrixBase< Derived > &x, const Eigen::MatrixBase< Derived > &y, const Eigen::MatrixBase< Derived > &z)
Definition: tools.h:54
hpp::fcl::Capsule::halfLength
FCL_REAL halfLength
Half Length along z axis.
Definition: shape/geometric_shapes.h:352
gjk.tf1
tuple tf1
Definition: test/scripts/gjk.py:28
hpp::fcl::ConvexBase::points
Vec3f * points
An array of the points of the polygon.
Definition: shape/geometric_shapes.h:623
hpp::fcl::details::GJK::evaluate
Status evaluate(const MinkowskiDiff &shape, const Vec3f &guess, const support_func_guess_t &supportHint=support_func_guess_t::Zero())
GJK algorithm, given the initial value guess.
Definition: src/narrowphase/gjk.cpp:624

hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:01