gjk_libccd-inl.h
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-2016, 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 #ifndef FCL_NARROWPHASE_DETAIL_GJKLIBCCD_INL_H
39 #define FCL_NARROWPHASE_DETAIL_GJKLIBCCD_INL_H
40 
43 
44 #include <array>
45 #include <unordered_map>
46 #include <unordered_set>
47 
48 #include "fcl/common/unused.h"
49 #include "fcl/common/warning.h"
50 
51 namespace fcl
52 {
53 
54 namespace detail
55 {
56 
57 //==============================================================================
58 extern template
59 class FCL_EXPORT GJKInitializer<double, Cylinder<double>>;
60 
61 //==============================================================================
62 extern template
63 class FCL_EXPORT GJKInitializer<double, Sphere<double>>;
64 
65 //==============================================================================
66 extern template
67 class FCL_EXPORT GJKInitializer<double, Ellipsoid<double>>;
68 
69 //==============================================================================
70 extern template
71 class FCL_EXPORT GJKInitializer<double, Box<double>>;
72 
73 //==============================================================================
74 extern template
75 class FCL_EXPORT GJKInitializer<double, Capsule<double>>;
76 
77 //==============================================================================
78 extern template
79 class FCL_EXPORT GJKInitializer<double, Cone<double>>;
80 
81 //==============================================================================
82 extern template
83 class FCL_EXPORT GJKInitializer<double, Convex<double>>;
84 
85 //==============================================================================
86 extern template
87 void* triCreateGJKObject(
88  const Vector3d& P1, const Vector3d& P2, const Vector3d& P3);
89 
90 //==============================================================================
91 extern template
92 void* triCreateGJKObject(
93  const Vector3d& P1,
94  const Vector3d& P2,
95  const Vector3d& P3,
96  const Transform3d& tf);
97 
98 //==============================================================================
99 extern template
100 bool GJKCollide(
101  void* obj1,
102  ccd_support_fn supp1,
103  ccd_center_fn cen1,
104  void* obj2,
105  ccd_support_fn supp2,
106  ccd_center_fn cen2,
107  unsigned int max_iterations,
108  double tolerance,
109  Vector3d* contact_points,
110  double* penetration_depth,
111  Vector3d* normal);
112 
113 //==============================================================================
114 extern template
115 bool GJKDistance(
116  void* obj1,
117  ccd_support_fn supp1,
118  void* obj2,
119  ccd_support_fn supp2,
120  unsigned int max_iterations,
121  double tolerance,
122  double* dist,
123  Vector3d* p1,
124  Vector3d* p2);
125 
126 extern template
127 bool GJKSignedDistance(
128  void* obj1,
129  ccd_support_fn supp1,
130  void* obj2,
131  ccd_support_fn supp2,
132  unsigned int max_iterations,
133  double tolerance,
134  double* dist,
135  Vector3d* p1,
136  Vector3d* p2);
137 
138 struct ccd_obj_t
139 {
140  ccd_vec3_t pos;
141  ccd_quat_t rot, rot_inv;
142 };
143 
144 struct ccd_box_t : public ccd_obj_t
145 {
146  ccd_real_t dim[3];
147 };
148 
149 struct ccd_cap_t : public ccd_obj_t
150 {
151  ccd_real_t radius, height;
152 };
153 
154 struct ccd_cyl_t : public ccd_obj_t
155 {
156  ccd_real_t radius, height;
157 };
158 
159 struct ccd_cone_t : public ccd_obj_t
160 {
161  ccd_real_t radius, height;
162 };
163 
164 struct ccd_sphere_t : public ccd_obj_t
165 {
166  ccd_real_t radius;
167 };
168 
169 struct ccd_ellipsoid_t : public ccd_obj_t
170 {
171  ccd_real_t radii[3];
172 };
173 
174 template <typename S>
175 struct ccd_convex_t : public ccd_obj_t
176 {
178 };
179 
180 struct ccd_triangle_t : public ccd_obj_t
181 {
182  ccd_vec3_t p[3];
183  ccd_vec3_t c;
184 };
185 
186 namespace libccd_extension
187 {
188 
189 static ccd_real_t simplexReduceToTriangle(ccd_simplex_t *simplex,
190  ccd_real_t dist,
191  ccd_vec3_t *best_witness)
192 {
193  ccd_real_t newdist;
194  ccd_vec3_t witness;
195  int best = -1;
196  int i;
197 
198  // try the fourth point in all three positions
199  for (i = 0; i < 3; i++){
200  newdist = ccdVec3PointTriDist2(ccd_vec3_origin,
201  &ccdSimplexPoint(simplex, (i == 0 ? 3 : 0))->v,
202  &ccdSimplexPoint(simplex, (i == 1 ? 3 : 1))->v,
203  &ccdSimplexPoint(simplex, (i == 2 ? 3 : 2))->v,
204  &witness);
205  newdist = CCD_SQRT(newdist);
206 
207  // record the best triangle
208  if (newdist < dist){
209  dist = newdist;
210  best = i;
211  ccdVec3Copy(best_witness, &witness);
212  }
213  }
214 
215  if (best >= 0){
216  ccdSimplexSet(simplex, best, ccdSimplexPoint(simplex, 3));
217  }
218  ccdSimplexSetSize(simplex, 3);
219 
220  return dist;
221 }
222 
223 _ccd_inline void tripleCross(const ccd_vec3_t *a, const ccd_vec3_t *b,
224  const ccd_vec3_t *c, ccd_vec3_t *d)
225 {
226  ccd_vec3_t e;
227  ccdVec3Cross(&e, a, b);
228  ccdVec3Cross(d, &e, c);
229 }
230 
231 /* This is *not* an implementation of the general function: what's the nearest
232  point on the line segment AB to the origin O? It is not intended to be.
233  This is a limited, special case which exploits the known (or at least
234  expected) construction history of AB. The history is as follows:
235 
236  1. We originally started with the Minkowski support point B (p_OB), which
237  was *not* the origin.
238  2. We define a support direction as p_BO = -p_OB and use that to get the
239  Minkowski support point A.
240  3. We confirm that O is not strictly beyond A in the direction p_BO
241  (confirming separation).
242  4. Then, and only then, do we invoke this method.
243 
244  The method will do one of two things:
245 
246  - determine if the origin lies within the simplex (i.e. lies on the line
247  segment, confirming non-separation) and reports if this is the case,
248  - otherwise it computes a new support direction: a vector pointing to the
249  origin from the nearest point on the segment AB. The direction is
250  guaranteed; the only guarantee about the magnitude is that it is
251  numerically viable (i.e. greater than epsilon).
252 
253  The algorithm exploits the construction history as outlined below. Without
254  loss of generality, we place B some non-zero distance away from the origin
255  along the î direction (all other orientations can be rigidly transformed to
256  this canonical example). The diagram below shows the origin O and the point
257  B. It also shows three regions: 1, 2, and 3.
258 
259 
260  1 ⯅ 2 3
261  │░░░░░░░░░░┆▒▒▒▒▒
262  │░░░░░░░░░░┆▒▒▒▒▒
263  │░░░░░░░░░░┆▒▒▒▒▒
264  │░░░░░░░░░░┆▒▒▒▒▒
265  │░░░░░░░░░░┆▒▒▒▒▒
266  ───────────O──────────B────⯈ î
267  │░░░░░░░░░░┆▒▒▒▒▒
268  │░░░░░░░░░░┆▒▒▒▒▒
269  │░░░░░░░░░░┆▒▒▒▒▒
270  │░░░░░░░░░░┆▒▒▒▒▒
271  │░░░░░░░░░░┆▒▒▒▒▒
272 
273  The point A cannot lie in region 3.
274 
275  - B is a support point of the Minkowski difference. p_BO defines the
276  support vector that produces the support point A. Vertex A must lie at
277  least as far in the p_BO as B otherwise A is not actually a valid support
278  point for that direction. It could lie on the boundary of regions 2 & 3
279  and still be a valid support point.
280 
281  The point A cannot lie in region 2 (or on the boundary between 2 & 3).
282  - We confirm that O is not strictly beyond A in the direction p_BO. For all
283  A in region 2, O lies beyond A (when projected onto the p_BO vector).
284 
285  The point A _must_ lie in region 1 (including the boundary between regions 1 &
286  2) by process of elimination.
287 
288  The implication of this is that the O must project onto the _interior_ of the
289  line segment AB (with one notable exception). If A = O, then the projection of
290  O is at the end point and, is in fact, itself.
291 
292  Therefore, this function can only have two possible outcomes:
293 
294  1. In the case where p_BA = k⋅p_BO (i.e., they are co-linear), we know the
295  origin lies "in" the simplex. If A = O, it lies on the simplex's surface
296  and the objects are touching, otherwise, the objects are penetrating.
297  Either way, we can report that they are definitely *not* separated.
298  2. p_BA ≠ k⋅p_BO, we define the new support direction as perpendicular to the
299  line segment AB, pointing to O from the nearest point on the segment to O.
300 
301  Return value indicates concrete knowledge that the origin lies "in" the
302  2-simplex (encoded as a 1), or indication that computation should continue (0).
303 
304  @note: doSimplex2 should _only_ be called with the construction history
305  outlined above: promotion of a 1-simplex. doSimplex2() is only invoked by
306  doSimplex(). This follows the computation of A and the promotion of the
307  simplex. Therefore, the history is always valid. Even though doSimplex3() can
308  demote itself to a 2-simplex, that 2-simplex immediately gets promoted back to
309  a 3-simplex via the same construction process. Therefore, as long as
310  doSimplex2() is only called from doSimplex() its region 1 assumption _should_
311  remain valid.
312 */
313 static int doSimplex2(ccd_simplex_t *simplex, ccd_vec3_t *dir) {
314  // Used to define numerical thresholds near zero; typically scaled to the size
315  // of the quantities being tested.
316  constexpr ccd_real_t eps = constants<ccd_real_t>::eps();
317 
318  const Vector3<ccd_real_t> p_OA(simplex->ps[simplex->last].v.v);
319  const Vector3<ccd_real_t> p_OB(simplex->ps[0].v.v);
320 
321  // Confirm that A is in region 1. Given that A may be very near to the origin,
322  // we must avoid normalizing p_OA. So, we use this instead.
323  // let A' be the projection of A onto the line defined by O and B.
324  // |A'B| >= |OB| iff A is in region 1.
325  // Numerically, we can express it as follows (allowing for |A'B| to be ever
326  // so slightly *less* than |OB|):
327  // p_AB ⋅ phat_OB >= |p_OB| - |p_OB| * ε = |p_OB| * (1 - ε)
328  // p_AB ⋅ phat_OB ⋅ |p_OB| >= |p_OB|⋅|p_OB| * (1 - ε)
329  // p_AB ⋅ p_OB >= |p_OB|² * (1 - ε)
330  // (p_OB - p_OA) ⋅ p_OB >= |p_OB|² * (1 - ε)
331  // p_OB ⋅ p_OB - p_OA ⋅ p_OB >= |p_OB|² * (1 - ε)
332  // |p_OB|² - p_OA ⋅ p_OB >= |p_OB|² * (1 - ε)
333  // -p_OA ⋅ p_OB >= -|p_OB|²ε
334  // p_OA ⋅ p_OB <= |p_OB|²ε
335  assert(p_OA.dot(p_OB) <= p_OB.squaredNorm() * eps && "A is not in region 1");
336 
337  // Test for co-linearity. Given A is in region 1, co-linearity --> O is "in"
338  // the simplex.
339  // We'll use the angle between two vectors to determine co-linearity: p_AB
340  // and p_OB. If they are co-linear, then the angle between them (θ) is zero.
341  // Similarly, sin(θ) is zero. Ideally, it can be expressed as:
342  // |p_AB × p_OB| = |p_AB||p_OB||sin(θ)| = 0
343  // Numerically, we allow θ (and sin(θ)) to be slightly larger than zero
344  // leading to a numerical formulation as:
345  // |p_AB × p_OB| = |p_AB||p_OB||sin(θ)| < |p_AB||p_OB|ε
346  // Finally, to reduce the computational cost, we eliminate the square roots by
347  // evaluating the equivalently discriminating test:
348  // |p_AB × p_OB|² < |p_AB|²|p_OB|²ε²
349  //
350  // In addition to providing a measure of co-linearity, the cross product gives
351  // us the normal to the plane on which points A, B, and O lie (which we will
352  // use later to compute a new support direction, as necessary).
353  const Vector3<ccd_real_t> p_AB = p_OB - p_OA;
354  const Vector3<ccd_real_t> plane_normal = p_OB.cross(p_AB);
355  if (plane_normal.squaredNorm() <
356  p_AB.squaredNorm() * p_OB.squaredNorm() * eps * eps) {
357  return 1;
358  }
359 
360  // O is not co-linear with AB, so dist(O, AB) > ε. Define `dir` as the
361  // direction to O from the nearest point on AB.
362  // Note: We use the normalized `plane_normal` (n̂) because we've already
363  // concluded that the origin is farther from AB than ε. We want to make sure
364  // `dir` likewise has a magnitude larger than ε. With normalization, we know
365  // |dir| = |n̂ × AB| = |AB| > dist(O, AB) > ε.
366  // Without normalizing, if |OA| and |OB| were smaller than ³√ε but
367  // sufficiently larger than ε, dist(O, AB) > ε, but |dir| < ε.
368  const Vector3<ccd_real_t> new_dir = plane_normal.normalized().cross(p_AB);
369  ccdVec3Set(dir, new_dir(0), new_dir(1), new_dir(2));
370  return 0;
371 }
372 
373 // Compares the given `value` against a _squared epsilon_. This is particularly
374 // important when testing some quantity (e.g., distance) to see if it
375 // is _functionally_ zero but using its _squared_ value in the test. Comparing
376 // _squared distance_ directly against epsilon is equivalent to comparing
377 // distance to sqrt(epsilon) -- we classify the distance as zero or not using
378 // only half the available precision.
379 static bool isAbsValueLessThanEpsSquared(ccd_real_t val) {
380  return std::abs(val) < std::numeric_limits<ccd_real_t>::epsilon() *
382 }
383 
384 // TODO(SeanCurtis-TRI): Define the return value:
385 // 1: (like doSimplex2) --> origin is "in" the simplex.
386 // 0:
387 // -1: If the 3-simplex is degenerate. How is this intepreted?
388 static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir)
389 {
390  const ccd_support_t *A, *B, *C;
391  ccd_vec3_t AO, AB, AC, ABC, tmp;
392  ccd_real_t dot;
393 
394  // get last added as A
395  A = ccdSimplexLast(simplex);
396  // get the other points
397  B = ccdSimplexPoint(simplex, 1);
398  C = ccdSimplexPoint(simplex, 0);
399 
400  // check touching contact
401  // Compute origin_projection as well. Without computing the origin projection,
402  // libccd could give inaccurate result. See
403  // https://github.com/danfis/libccd/issues/55.
404  ccd_vec3_t origin_projection_unused;
405 
406  const ccd_real_t dist_squared = ccdVec3PointTriDist2(
407  ccd_vec3_origin, &A->v, &B->v, &C->v, &origin_projection_unused);
408  if (isAbsValueLessThanEpsSquared(dist_squared)) {
409  return 1;
410  }
411 
412  // check if triangle is really triangle (has area > 0)
413  // if not simplex can't be expanded and thus no intersection is found
414  // TODO(SeanCurtis-TRI): Coincident points is sufficient but not necessary
415  // for a zero-area triangle. What about co-linearity? Can we guarantee that
416  // co-linearity can't happen? See the `triangle_area_is_zero()` method in
417  // this same file.
418  if (ccdVec3Eq(&A->v, &B->v) || ccdVec3Eq(&A->v, &C->v)){
419  // TODO(SeanCurtis-TRI): Why do we simply quit if the simplex is degenerate?
420  return -1;
421  }
422 
423  // compute AO vector
424  ccdVec3Copy(&AO, &A->v);
425  ccdVec3Scale(&AO, -CCD_ONE);
426 
427  // compute AB and AC segments and ABC vector (perpendircular to triangle)
428  ccdVec3Sub2(&AB, &B->v, &A->v);
429  ccdVec3Sub2(&AC, &C->v, &A->v);
430  ccdVec3Cross(&ABC, &AB, &AC);
431 
432  ccdVec3Cross(&tmp, &ABC, &AC);
433  dot = ccdVec3Dot(&tmp, &AO);
434  if (ccdIsZero(dot) || dot > CCD_ZERO){
435  dot = ccdVec3Dot(&AC, &AO);
436  if (ccdIsZero(dot) || dot > CCD_ZERO){
437  // C is already in place
438  ccdSimplexSet(simplex, 1, A);
439  ccdSimplexSetSize(simplex, 2);
440  tripleCross(&AC, &AO, &AC, dir);
441  }else{
442  ccd_do_simplex3_45:
443  dot = ccdVec3Dot(&AB, &AO);
444  if (ccdIsZero(dot) || dot > CCD_ZERO){
445  ccdSimplexSet(simplex, 0, B);
446  ccdSimplexSet(simplex, 1, A);
447  ccdSimplexSetSize(simplex, 2);
448  tripleCross(&AB, &AO, &AB, dir);
449  }else{
450  ccdSimplexSet(simplex, 0, A);
451  ccdSimplexSetSize(simplex, 1);
452  ccdVec3Copy(dir, &AO);
453  }
454  }
455  }else{
456  ccdVec3Cross(&tmp, &AB, &ABC);
457  dot = ccdVec3Dot(&tmp, &AO);
458  if (ccdIsZero(dot) || dot > CCD_ZERO){
459  goto ccd_do_simplex3_45;
460  }else{
461  dot = ccdVec3Dot(&ABC, &AO);
462  if (ccdIsZero(dot) || dot > CCD_ZERO){
463  ccdVec3Copy(dir, &ABC);
464  }else{
465  ccd_support_t Ctmp;
466  ccdSupportCopy(&Ctmp, C);
467  ccdSimplexSet(simplex, 0, B);
468  ccdSimplexSet(simplex, 1, &Ctmp);
469 
470  ccdVec3Copy(dir, &ABC);
471  ccdVec3Scale(dir, -CCD_ONE);
472  }
473  }
474  }
475 
476  return 0;
477 }
478 
479 static int doSimplex4(ccd_simplex_t *simplex, ccd_vec3_t *dir)
480 {
481  const ccd_support_t *A, *B, *C, *D;
482  ccd_vec3_t AO, AB, AC, AD, ABC, ACD, ADB;
483  int B_on_ACD, C_on_ADB, D_on_ABC;
484  int AB_O, AC_O, AD_O;
485 
486  // get last added as A
487  A = ccdSimplexLast(simplex);
488  // get the other points
489  B = ccdSimplexPoint(simplex, 2);
490  C = ccdSimplexPoint(simplex, 1);
491  D = ccdSimplexPoint(simplex, 0);
492 
493  // check if tetrahedron is really tetrahedron (has volume > 0)
494  // if it is not simplex can't be expanded and thus no intersection is
495  // found.
496  // point_projection_on_triangle_unused is not used. We ask
497  // ccdVec3PointTriDist2 to compute this witness point, so as to get a
498  // numerical robust dist_squared. See
499  // https://github.com/danfis/libccd/issues/55 for an explanation.
500  ccd_vec3_t point_projection_on_triangle_unused;
501  ccd_real_t dist_squared = ccdVec3PointTriDist2(
502  &A->v, &B->v, &C->v, &D->v, &point_projection_on_triangle_unused);
503  if (isAbsValueLessThanEpsSquared(dist_squared)) {
504  return -1;
505  }
506 
507  // check if origin lies on some of tetrahedron's face - if so objects
508  // intersect
509  dist_squared = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v,
510  &point_projection_on_triangle_unused);
511  if (isAbsValueLessThanEpsSquared((dist_squared))) return 1;
512  dist_squared = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &C->v, &D->v,
513  &point_projection_on_triangle_unused);
514  if (isAbsValueLessThanEpsSquared((dist_squared))) return 1;
515  dist_squared = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &D->v,
516  &point_projection_on_triangle_unused);
517  if (isAbsValueLessThanEpsSquared(dist_squared)) return 1;
518  dist_squared = ccdVec3PointTriDist2(ccd_vec3_origin, &B->v, &C->v, &D->v,
519  &point_projection_on_triangle_unused);
520  if (isAbsValueLessThanEpsSquared(dist_squared)) return 1;
521 
522  // compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors
523  ccdVec3Copy(&AO, &A->v);
524  ccdVec3Scale(&AO, -CCD_ONE);
525  ccdVec3Sub2(&AB, &B->v, &A->v);
526  ccdVec3Sub2(&AC, &C->v, &A->v);
527  ccdVec3Sub2(&AD, &D->v, &A->v);
528  ccdVec3Cross(&ABC, &AB, &AC);
529  ccdVec3Cross(&ACD, &AC, &AD);
530  ccdVec3Cross(&ADB, &AD, &AB);
531 
532  // side (positive or negative) of B, C, D relative to planes ACD, ADB
533  // and ABC respectively
534  B_on_ACD = ccdSign(ccdVec3Dot(&ACD, &AB));
535  C_on_ADB = ccdSign(ccdVec3Dot(&ADB, &AC));
536  D_on_ABC = ccdSign(ccdVec3Dot(&ABC, &AD));
537 
538  // whether origin is on same side of ACD, ADB, ABC as B, C, D
539  // respectively
540  AB_O = ccdSign(ccdVec3Dot(&ACD, &AO)) == B_on_ACD;
541  AC_O = ccdSign(ccdVec3Dot(&ADB, &AO)) == C_on_ADB;
542  AD_O = ccdSign(ccdVec3Dot(&ABC, &AO)) == D_on_ABC;
543 
544  if (AB_O && AC_O && AD_O){
545  // origin is in tetrahedron
546  return 1;
547 
548  // rearrange simplex to triangle and call doSimplex3()
549  }else if (!AB_O){
550  // B is farthest from the origin among all of the tetrahedron's
551  // points, so remove it from the list and go on with the triangle
552  // case
553 
554  // D and C are in place
555  ccdSimplexSet(simplex, 2, A);
556  ccdSimplexSetSize(simplex, 3);
557  }else if (!AC_O){
558  // C is farthest
559  ccdSimplexSet(simplex, 1, D);
560  ccdSimplexSet(simplex, 0, B);
561  ccdSimplexSet(simplex, 2, A);
562  ccdSimplexSetSize(simplex, 3);
563  }else{ // (!AD_O)
564  ccdSimplexSet(simplex, 0, C);
565  ccdSimplexSet(simplex, 1, B);
566  ccdSimplexSet(simplex, 2, A);
567  ccdSimplexSetSize(simplex, 3);
568  }
569 
570  return doSimplex3(simplex, dir);
571 }
572 
573 static int doSimplex(ccd_simplex_t *simplex, ccd_vec3_t *dir)
574 {
575  if (ccdSimplexSize(simplex) == 2){
576  // simplex contains segment only one segment
577  return doSimplex2(simplex, dir);
578  }else if (ccdSimplexSize(simplex) == 3){
579  // simplex contains triangle
580  return doSimplex3(simplex, dir);
581  }else{ // ccdSimplexSize(simplex) == 4
582  // tetrahedron - this is the only shape which can encapsule origin
583  // so doSimplex4() also contains test on it
584  return doSimplex4(simplex, dir);
585  }
586 }
587 
589 static int simplexToPolytope2(const void *obj1, const void *obj2,
590  const ccd_t *ccd,
591  const ccd_simplex_t *simplex,
592  ccd_pt_t *pt, ccd_pt_el_t **nearest)
593 {
594  const ccd_support_t *a, *b;
595  ccd_vec3_t ab, ac, dir;
596  ccd_support_t supp[4];
597  ccd_pt_vertex_t *v[6];
598  ccd_pt_edge_t *e[12];
599  size_t i;
600  int found;
601 
602  a = ccdSimplexPoint(simplex, 0);
603  b = ccdSimplexPoint(simplex, 1);
604 
605  // This situation is a bit tricky. If only one segment comes from
606  // previous run of GJK - it means that either this segment is on
607  // minkowski edge (and thus we have touch contact) or it it isn't and
608  // therefore segment is somewhere *inside* minkowski sum and it *must*
609  // be possible to fully enclose this segment with polyhedron formed by
610  // at least 8 triangle faces.
611 
612  // get first support point (any)
613  found = 0;
614  for (i = 0; i < ccd_points_on_sphere_len; i++){
615  __ccdSupport(obj1, obj2, &ccd_points_on_sphere[i], ccd, &supp[0]);
616  if (!ccdVec3Eq(&a->v, &supp[0].v) && !ccdVec3Eq(&b->v, &supp[0].v)){
617  found = 1;
618  break;
619  }
620  }
621  if (!found)
622  goto simplexToPolytope2_touching_contact;
623 
624  // get second support point in opposite direction than supp[0]
625  ccdVec3Copy(&dir, &supp[0].v);
626  ccdVec3Scale(&dir, -CCD_ONE);
627  __ccdSupport(obj1, obj2, &dir, ccd, &supp[1]);
628  if (ccdVec3Eq(&a->v, &supp[1].v) || ccdVec3Eq(&b->v, &supp[1].v))
629  goto simplexToPolytope2_touching_contact;
630 
631  // next will be in direction of normal of triangle a,supp[0],supp[1]
632  ccdVec3Sub2(&ab, &supp[0].v, &a->v);
633  ccdVec3Sub2(&ac, &supp[1].v, &a->v);
634  ccdVec3Cross(&dir, &ab, &ac);
635  __ccdSupport(obj1, obj2, &dir, ccd, &supp[2]);
636  if (ccdVec3Eq(&a->v, &supp[2].v) || ccdVec3Eq(&b->v, &supp[2].v))
637  goto simplexToPolytope2_touching_contact;
638 
639  // and last one will be in opposite direction
640  ccdVec3Scale(&dir, -CCD_ONE);
641  __ccdSupport(obj1, obj2, &dir, ccd, &supp[3]);
642  if (ccdVec3Eq(&a->v, &supp[3].v) || ccdVec3Eq(&b->v, &supp[3].v))
643  goto simplexToPolytope2_touching_contact;
644 
645  goto simplexToPolytope2_not_touching_contact;
646 simplexToPolytope2_touching_contact:
647  v[0] = ccdPtAddVertex(pt, a);
648  v[1] = ccdPtAddVertex(pt, b);
649  *nearest = (ccd_pt_el_t *)ccdPtAddEdge(pt, v[0], v[1]);
650  if (*nearest == NULL)
651  return -2;
652 
653  return -1;
654 
655 simplexToPolytope2_not_touching_contact:
656  // form polyhedron
657  v[0] = ccdPtAddVertex(pt, a);
658  v[1] = ccdPtAddVertex(pt, &supp[0]);
659  v[2] = ccdPtAddVertex(pt, b);
660  v[3] = ccdPtAddVertex(pt, &supp[1]);
661  v[4] = ccdPtAddVertex(pt, &supp[2]);
662  v[5] = ccdPtAddVertex(pt, &supp[3]);
663 
664  e[0] = ccdPtAddEdge(pt, v[0], v[1]);
665  e[1] = ccdPtAddEdge(pt, v[1], v[2]);
666  e[2] = ccdPtAddEdge(pt, v[2], v[3]);
667  e[3] = ccdPtAddEdge(pt, v[3], v[0]);
668 
669  e[4] = ccdPtAddEdge(pt, v[4], v[0]);
670  e[5] = ccdPtAddEdge(pt, v[4], v[1]);
671  e[6] = ccdPtAddEdge(pt, v[4], v[2]);
672  e[7] = ccdPtAddEdge(pt, v[4], v[3]);
673 
674  e[8] = ccdPtAddEdge(pt, v[5], v[0]);
675  e[9] = ccdPtAddEdge(pt, v[5], v[1]);
676  e[10] = ccdPtAddEdge(pt, v[5], v[2]);
677  e[11] = ccdPtAddEdge(pt, v[5], v[3]);
678 
679  if (ccdPtAddFace(pt, e[4], e[5], e[0]) == NULL
680  || ccdPtAddFace(pt, e[5], e[6], e[1]) == NULL
681  || ccdPtAddFace(pt, e[6], e[7], e[2]) == NULL
682  || ccdPtAddFace(pt, e[7], e[4], e[3]) == NULL
683 
684  || ccdPtAddFace(pt, e[8], e[9], e[0]) == NULL
685  || ccdPtAddFace(pt, e[9], e[10], e[1]) == NULL
686  || ccdPtAddFace(pt, e[10], e[11], e[2]) == NULL
687  || ccdPtAddFace(pt, e[11], e[8], e[3]) == NULL){
688  return -2;
689  }
690 
691  return 0;
692 }
693 
694 #ifndef NDEBUG
695 static bool isPolytopeEmpty(const ccd_pt_t& polytope) {
696  ccd_pt_vertex_t* v = nullptr;
697  ccdListForEachEntry(&polytope.vertices, v, ccd_pt_vertex_t, list) {
698  if (v) {
699  return false;
700  }
701  }
702  ccd_pt_edge_t* e = nullptr;
703  ccdListForEachEntry(&polytope.edges, e, ccd_pt_edge_t, list) {
704  if (e) {
705  return false;
706  }
707  }
708  ccd_pt_face_t* f = nullptr;
709  ccdListForEachEntry(&polytope.faces, f, ccd_pt_face_t, list) {
710  if (f) {
711  return false;
712  }
713  }
714  return true;
715 }
716 #endif
717 
735 static int convert2SimplexToTetrahedron(const void* obj1, const void* obj2,
736  const ccd_t* ccd, const ccd_simplex_t* simplex,
737  ccd_pt_t* polytope, ccd_pt_el_t** nearest) {
738  assert(nearest);
739  assert(isPolytopeEmpty(*polytope));
740  assert(simplex->last == 2); // a 2-simplex.
741  const ccd_support_t *a, *b, *c;
742  ccd_support_t d, d2;
743  ccd_vec3_t ab, ac, dir;
744  ccd_pt_vertex_t* v[4];
745  ccd_pt_edge_t* e[6];
746 
747  *nearest = NULL;
748 
749  a = ccdSimplexPoint(simplex, 0);
750  b = ccdSimplexPoint(simplex, 1);
751  c = ccdSimplexPoint(simplex, 2);
752 
753  // The 2-simplex is just a triangle containing the origin. We will expand this
754  // triangle to a tetrahedron, by adding the support point along the normal
755  // direction of the triangle.
756  ccdVec3Sub2(&ab, &b->v, &a->v);
757  ccdVec3Sub2(&ac, &c->v, &a->v);
758  ccdVec3Cross(&dir, &ab, &ac);
759  __ccdSupport(obj1, obj2, &dir, ccd, &d);
760  ccd_vec3_t point_projection_on_triangle_unused;
761  const ccd_real_t dist_squared = ccdVec3PointTriDist2(
762  &d.v, &a->v, &b->v, &c->v, &point_projection_on_triangle_unused);
763 
764  // and second one take in opposite direction
765  ccdVec3Scale(&dir, -CCD_ONE);
766  __ccdSupport(obj1, obj2, &dir, ccd, &d2);
767  const ccd_real_t dist_squared_opposite = ccdVec3PointTriDist2(
768  &d2.v, &a->v, &b->v, &c->v, &point_projection_on_triangle_unused);
769 
770  // check if face isn't already on edge of minkowski sum and thus we
771  // have touching contact
772  if (ccdIsZero(dist_squared) || ccdIsZero(dist_squared_opposite)) {
773  v[0] = ccdPtAddVertex(polytope, a);
774  v[1] = ccdPtAddVertex(polytope, b);
775  v[2] = ccdPtAddVertex(polytope, c);
776  e[0] = ccdPtAddEdge(polytope, v[0], v[1]);
777  e[1] = ccdPtAddEdge(polytope, v[1], v[2]);
778  e[2] = ccdPtAddEdge(polytope, v[2], v[0]);
779  *nearest = (ccd_pt_el_t*)ccdPtAddFace(polytope, e[0], e[1], e[2]);
780  if (*nearest == NULL) return -2;
781 
782  return -1;
783  }
784  // Form a tetrahedron with abc as one face, pick either d or d2, based
785  // on which one has larger distance to the face abc. We pick the larger
786  // distance because it gives a tetrahedron with larger volume, so potentially
787  // more "expanded" than the one with the smaller volume.
788  auto FormTetrahedron = [polytope, a, b, c, &v,
789  &e](const ccd_support_t& new_support) -> int {
790  v[0] = ccdPtAddVertex(polytope, a);
791  v[1] = ccdPtAddVertex(polytope, b);
792  v[2] = ccdPtAddVertex(polytope, c);
793  v[3] = ccdPtAddVertex(polytope, &new_support);
794 
795  e[0] = ccdPtAddEdge(polytope, v[0], v[1]);
796  e[1] = ccdPtAddEdge(polytope, v[1], v[2]);
797  e[2] = ccdPtAddEdge(polytope, v[2], v[0]);
798  e[3] = ccdPtAddEdge(polytope, v[0], v[3]);
799  e[4] = ccdPtAddEdge(polytope, v[1], v[3]);
800  e[5] = ccdPtAddEdge(polytope, v[2], v[3]);
801 
802  // ccdPtAdd*() functions return NULL either if the memory allocation
803  // failed of if any of the input pointers are NULL, so the bad
804  // allocation can be checked by the last calls of ccdPtAddFace()
805  // because the rest of the bad allocations eventually "bubble up" here
806  // Note, there is no requirement on the winding of the face, namely we do
807  // not guarantee if all f.e(0).cross(f.e(1)) points outward (or inward) for
808  // all the faces added below.
809  if (ccdPtAddFace(polytope, e[0], e[1], e[2]) == NULL ||
810  ccdPtAddFace(polytope, e[3], e[4], e[0]) == NULL ||
811  ccdPtAddFace(polytope, e[4], e[5], e[1]) == NULL ||
812  ccdPtAddFace(polytope, e[5], e[3], e[2]) == NULL) {
813  return -2;
814  }
815  return 0;
816  };
817 
818  if (std::abs(dist_squared) > std::abs(dist_squared_opposite)) {
819  return FormTetrahedron(d);
820  } else {
821  return FormTetrahedron(d2);
822  }
823 }
824 
827 static int simplexToPolytope4(const void* obj1, const void* obj2,
828  const ccd_t* ccd, ccd_simplex_t* simplex,
829  ccd_pt_t* pt, ccd_pt_el_t** nearest) {
830  const ccd_support_t *a, *b, *c, *d;
831  bool use_polytope3{false};
832  ccd_pt_vertex_t* v[4];
833  ccd_pt_edge_t* e[6];
834  size_t i;
835 
836  a = ccdSimplexPoint(simplex, 0);
837  b = ccdSimplexPoint(simplex, 1);
838  c = ccdSimplexPoint(simplex, 2);
839  d = ccdSimplexPoint(simplex, 3);
840 
841  // The origin can lie on any of the tetrahedra faces. In fact, for a
842  // degenerate tetrahedron, it could be considered to lie on multiple faces
843  // simultaneously. If it lies on any face, we can simply reduce the dimension
844  // of the simplex to that face and then attempt to construct the polytope from
845  // the resulting face. We simply take the first face which exhibited the
846  // trait.
847  ccd_real_t dist_squared =
848  ccdVec3PointTriDist2(ccd_vec3_origin, &a->v, &b->v, &c->v, NULL);
849  if (isAbsValueLessThanEpsSquared(dist_squared)) {
850  use_polytope3 = true;
851  }
852  if (!use_polytope3) {
853  dist_squared =
854  ccdVec3PointTriDist2(ccd_vec3_origin, &a->v, &c->v, &d->v, NULL);
855  if (isAbsValueLessThanEpsSquared(dist_squared)) {
856  use_polytope3 = true;
857  ccdSimplexSet(simplex, 1, c);
858  ccdSimplexSet(simplex, 2, d);
859  }
860  }
861  if (!use_polytope3) {
862  dist_squared =
863  ccdVec3PointTriDist2(ccd_vec3_origin, &a->v, &b->v, &d->v, NULL);
864  if (isAbsValueLessThanEpsSquared(dist_squared)) {
865  use_polytope3 = true;
866  ccdSimplexSet(simplex, 2, d);
867  }
868  }
869  if (!use_polytope3) {
870  dist_squared =
871  ccdVec3PointTriDist2(ccd_vec3_origin, &b->v, &c->v, &d->v, NULL);
872  if (isAbsValueLessThanEpsSquared(dist_squared)) {
873  use_polytope3 = true;
874  ccdSimplexSet(simplex, 0, b);
875  ccdSimplexSet(simplex, 1, c);
876  ccdSimplexSet(simplex, 2, d);
877  }
878  }
879 
880  if (use_polytope3) {
881  ccdSimplexSetSize(simplex, 3);
882  return convert2SimplexToTetrahedron(obj1, obj2, ccd, simplex, pt, nearest);
883  }
884 
885  // no touching contact - simply create tetrahedron
886  for (i = 0; i < 4; i++) {
887  v[i] = ccdPtAddVertex(pt, ccdSimplexPoint(simplex, i));
888  }
889 
890  e[0] = ccdPtAddEdge(pt, v[0], v[1]);
891  e[1] = ccdPtAddEdge(pt, v[1], v[2]);
892  e[2] = ccdPtAddEdge(pt, v[2], v[0]);
893  e[3] = ccdPtAddEdge(pt, v[3], v[0]);
894  e[4] = ccdPtAddEdge(pt, v[3], v[1]);
895  e[5] = ccdPtAddEdge(pt, v[3], v[2]);
896 
897  // ccdPtAdd*() functions return NULL either if the memory allocation
898  // failed of if any of the input pointers are NULL, so the bad
899  // allocation can be checked by the last calls of ccdPtAddFace()
900  // because the rest of the bad allocations eventually "bubble up" here
901  if (ccdPtAddFace(pt, e[0], e[1], e[2]) == NULL ||
902  ccdPtAddFace(pt, e[3], e[4], e[0]) == NULL ||
903  ccdPtAddFace(pt, e[4], e[5], e[1]) == NULL ||
904  ccdPtAddFace(pt, e[5], e[3], e[2]) == NULL) {
905  return -2;
906  }
907 
908  return 0;
909 }
910 
912 static bool are_coincident(const ccd_vec3_t& p, const ccd_vec3_t& q) {
913  // This uses a scale-dependent basis for determining coincidence. It examines
914  // each axis independently, and only, if all three axes are sufficiently
915  // close (relative to its own scale), are the two points considered
916  // coincident.
917  //
918  // For dimension i, two values are considered the same if:
919  // |pᵢ - qᵢ| <= ε·max(1, |pᵢ|, |qᵢ|)
920  // And the points are coincident if the previous condition holds for all
921  // `i ∈ {0, 1, 2}` (i.e. the x-, y-, *and* z-dimensions).
922  using std::abs;
923  using std::max;
924 
925  constexpr ccd_real_t eps = constants<ccd_real_t>::eps();
926  // NOTE: Wrapping "1.0" with ccd_real_t accounts for mac problems where ccd
927  // is actually float based.
928  for (int i = 0; i < 3; ++i) {
929  const ccd_real_t tolerance =
930  max({ccd_real_t{1}, abs(p.v[i]), abs(q.v[i])}) * eps;
931  const ccd_real_t delta = abs(p.v[i] - q.v[i]);
932  if (delta > tolerance) return false;
933  }
934  return true;
935 }
936 
943 static bool triangle_area_is_zero(const ccd_vec3_t& a, const ccd_vec3_t& b,
944  const ccd_vec3_t& c) {
945  // First coincidence condition. This doesn't *explicitly* test for b and c
946  // being coincident. That will be captured in the subsequent co-linearity
947  // test. If b and c *were* coincident, it would be cheaper to perform the
948  // coincidence test than the co-linearity test.
949  // However, the expectation is that typically the triangle will not have zero
950  // area. In that case, we want to minimize the number of tests performed on
951  // the average, so we prefer to eliminate one coincidence test.
952  if (are_coincident(a, b) || are_coincident(a, c)) return true;
953 
954  // We're going to compute the *sine* of the angle θ between edges (given that
955  // the vertices are *not* coincident). If the sin(θ) < ε, the edges are
956  // co-linear.
957  ccd_vec3_t AB, AC, n;
958  ccdVec3Sub2(&AB, &b, &a);
959  ccdVec3Sub2(&AC, &c, &a);
960  ccdVec3Normalize(&AB);
961  ccdVec3Normalize(&AC);
962  ccdVec3Cross(&n, &AB, &AC);
963  constexpr ccd_real_t eps = constants<ccd_real_t>::eps();
964  // Second co-linearity condition.
965  if (ccdVec3Len2(&n) < eps * eps) return true;
966  return false;
967 }
968 
982 static ccd_vec3_t faceNormalPointingOutward(const ccd_pt_t* polytope,
983  const ccd_pt_face_t* face) {
984  // This doesn't necessarily define a triangle; I don't know that the third
985  // vertex added here is unique from the other two.
986 #ifndef NDEBUG
987  // quick test for degeneracy
988  const ccd_vec3_t& a = face->edge[0]->vertex[1]->v.v;
989  const ccd_vec3_t& b = face->edge[0]->vertex[0]->v.v;
990  const ccd_vec3_t& test_v = face->edge[1]->vertex[0]->v.v;
991  const ccd_vec3_t& c = are_coincident(test_v, a) || are_coincident(test_v, b) ?
992  face->edge[1]->vertex[1]->v.v : test_v;
993  if (triangle_area_is_zero(a, b, c)) {
995  "Cannot compute face normal for a degenerate (zero-area) triangle");
996  }
997 #endif
998  // We find two edges of the triangle as e1 and e2, and the normal vector
999  // of the face is e1.cross(e2).
1000  ccd_vec3_t e1, e2;
1001  ccdVec3Sub2(&e1, &(face->edge[0]->vertex[1]->v.v),
1002  &(face->edge[0]->vertex[0]->v.v));
1003  ccdVec3Sub2(&e2, &(face->edge[1]->vertex[1]->v.v),
1004  &(face->edge[1]->vertex[0]->v.v));
1005  ccd_vec3_t dir;
1006  // TODO(hongkai.dai): we ignore the degeneracy here, namely we assume e1 and
1007  // e2 are not colinear. We should check if e1 and e2 are colinear, and handle
1008  // this corner case.
1009  ccdVec3Cross(&dir, &e1, &e2);
1010  const ccd_real_t dir_norm = std::sqrt(ccdVec3Len2(&dir));
1011  ccd_vec3_t unit_dir = dir;
1012  ccdVec3Scale(&unit_dir, 1.0 / dir_norm);
1013  // The winding of the triangle is *not* guaranteed. The normal `n = e₁ × e₂`
1014  // may point inside or outside. We rely on the fact that the origin lies
1015  // within the polytope to resolve this ambiguity. A vector from the origin to
1016  // any point on the triangle must point in the "same" direction as the normal
1017  // (positive dot product).
1018 
1019  // However, the distance to the origin may be too small for the origin to
1020  // serve as a reliable witness of inside-ness. In that case, we examine the
1021  // polytope's *other* vertices; they should all lie on the "inside" of the
1022  // current triangle. If at least one is a reliable distance, then that is
1023  // considered to be the inside. If all vertices are "too close" (like the
1024  // origin), then "inside" is defined as the side of the triangle that had the
1025  // most distant vertex.
1026 
1027  // For these tests, we use the arbitrary distance of 0.01 unit length as a
1028  // "reliable" distance for both the origin and other vertices. Even if it
1029  // seems large, the fall through case of comparing the maximum distance will
1030  // always guarantee correctness.
1031  const ccd_real_t dist_tol = 0.01;
1032  // origin_distance_to_plane computes the signed distance from the origin to
1033  // the plane nᵀ * (x - v) = 0 coinciding with the triangle, where v is a
1034  // point on the triangle.
1035  const ccd_real_t origin_distance_to_plane =
1036  ccdVec3Dot(&unit_dir, &(face->edge[0]->vertex[0]->v.v));
1037  if (origin_distance_to_plane < -dist_tol) {
1038  // Origin is more than `dist_tol` away from the plane, but the negative
1039  // value implies that the normal vector is pointing in the wrong direction;
1040  // flip it.
1041  ccdVec3Scale(&dir, ccd_real_t(-1));
1042  } else if (-dist_tol <= origin_distance_to_plane &&
1043  origin_distance_to_plane <= dist_tol) {
1044  // The origin is close to the plane of the face. Pick another vertex to test
1045  // the normal direction.
1046  ccd_real_t max_distance_to_plane = -CCD_REAL_MAX;
1047  ccd_real_t min_distance_to_plane = CCD_REAL_MAX;
1048  ccd_pt_vertex_t* v;
1049  // If the magnitude of the distance_to_plane is larger than dist_tol,
1050  // then it means one of the vertices is at least `dist_tol` away from the
1051  // plane coinciding with the face.
1052  ccdListForEachEntry(&polytope->vertices, v, ccd_pt_vertex_t, list) {
1053  // distance_to_plane is the signed distance from the
1054  // vertex v->v.v to the face, i.e., distance_to_plane = nᵀ *
1055  // (v->v.v - face_point). Note that origin_distance_to_plane = nᵀ *
1056  // face_point.
1057  const ccd_real_t distance_to_plane =
1058  ccdVec3Dot(&unit_dir, &(v->v.v)) - origin_distance_to_plane;
1059  if (distance_to_plane > dist_tol) {
1060  // The vertex is at least dist_tol away from the face plane, on the same
1061  // direction of `dir`. So we flip dir to point it outward from the
1062  // polytope.
1063  ccdVec3Scale(&dir, ccd_real_t(-1));
1064  return dir;
1065  } else if (distance_to_plane < -dist_tol) {
1066  // The vertex is at least `dist_tol` away from the face plane, on the
1067  // opposite direction of `dir`. So `dir` points outward already.
1068  return dir;
1069  } else {
1070  max_distance_to_plane =
1071  std::max(max_distance_to_plane, distance_to_plane);
1072  min_distance_to_plane =
1073  std::min(min_distance_to_plane, distance_to_plane);
1074  }
1075  }
1076  // If max_distance_to_plane > |min_distance_to_plane|, it means that the
1077  // vertices that are on the positive side of the plane, has a larger maximal
1078  // distance than the vertices on the negative side of the plane. Thus we
1079  // regard that `dir` points into the polytope. Hence we flip `dir`.
1080  if (max_distance_to_plane > std::abs(min_distance_to_plane)) {
1081  ccdVec3Scale(&dir, ccd_real_t(-1));
1082  }
1083  }
1084  return dir;
1085 }
1086 
1087 // Return true if the point `pt` is on the outward side of the half-plane, on
1088 // which the triangle `f1 lives. Notice if the point `pt` is exactly on the
1089 // half-plane, the return is false.
1090 // @param f A triangle on a polytope.
1091 // @param pt A point.
1092 static bool isOutsidePolytopeFace(const ccd_pt_t* polytope,
1093  const ccd_pt_face_t* f, const ccd_vec3_t* pt) {
1094  ccd_vec3_t n = faceNormalPointingOutward(polytope, f);
1095  // r_VP is the vector from a vertex V on the face `f`, to the point P `pt`.
1096  ccd_vec3_t r_VP;
1097  ccdVec3Sub2(&r_VP, pt, &(f->edge[0]->vertex[0]->v.v));
1098  return ccdVec3Dot(&n, &r_VP) > 0;
1099 }
1100 
1101 #ifndef NDEBUG
1102 // The function ComputeVisiblePatchRecursiveSanityCheck() is only called in the
1103 // debug mode. In the release mode, this function is declared/defined but not
1104 // used. Without this NDEBUG macro, the function will cause a -Wunused-function
1105 // error on CI's release builds.
1116  const ccd_pt_t& polytope,
1117  const std::unordered_set<ccd_pt_edge_t*>& border_edges,
1118  const std::unordered_set<ccd_pt_face_t*>& visible_faces,
1119  const std::unordered_set<ccd_pt_edge_t*>& internal_edges) {
1120  ccd_pt_face_t* f;
1121  // TODO(SeanCurtis-TRI): Instead of returning false, have this throw the
1122  // exception itself so that it can provide additional information.
1123  ccdListForEachEntry(&polytope.faces, f, ccd_pt_face_t, list) {
1124  bool has_edge_internal = false;
1125  for (int i = 0; i < 3; ++i) {
1126  // Since internal_edges is a set, internal_edges.count(e) means that e
1127  // is contained in the set internal_edges.
1128  if (internal_edges.count(f->edge[i]) > 0) {
1129  has_edge_internal = true;
1130  break;
1131  }
1132  }
1133  if (has_edge_internal) {
1134  if (visible_faces.count(f) == 0) {
1135  return false;
1136  }
1137  }
1138  }
1139  ccd_pt_edge_t* e;
1140  ccdListForEachEntry(&polytope.edges, e, ccd_pt_edge_t, list) {
1141  if (visible_faces.count(e->faces[0]) > 0 &&
1142  visible_faces.count(e->faces[1]) > 0) {
1143  if (internal_edges.count(e) == 0) {
1144  return false;
1145  }
1146  } else if (visible_faces.count(e->faces[0]) +
1147  visible_faces.count(e->faces[1]) ==
1148  1) {
1149  if (border_edges.count(e) == 0) {
1150  return false;
1151  }
1152  }
1153  }
1154  for (const auto b : border_edges) {
1155  if (internal_edges.count(b) > 0) {
1156  return false;
1157  }
1158  }
1159  return true;
1160 }
1161 #endif
1162 
1171  std::unordered_set<ccd_pt_edge_t*>* border_edges,
1172  std::unordered_set<ccd_pt_edge_t*>* internal_edges) {
1173  border_edges->insert(edge);
1174  if (internal_edges->count(edge) > 0) {
1176  "An edge is being classified as border that has already been "
1177  "classifed as internal");
1178  }
1179 }
1180 
1189  std::unordered_set<ccd_pt_edge_t*>* border_edges,
1190  std::unordered_set<ccd_pt_edge_t*>* internal_edges) {
1191  internal_edges->insert(edge);
1192  if (border_edges->count(edge) > 0) {
1194  "An edge is being classified as internal that has already been "
1195  "classified as border");
1196  }
1197 }
1198 
1213  const ccd_pt_t& polytope, ccd_pt_face_t& f, int edge_index,
1214  const ccd_vec3_t& query_point,
1215  std::unordered_set<ccd_pt_edge_t*>* border_edges,
1216  std::unordered_set<ccd_pt_face_t*>* visible_faces,
1217  std::unordered_set<ccd_pt_face_t*>* hidden_faces,
1218  std::unordered_set<ccd_pt_edge_t*>* internal_edges) {
1219  ccd_pt_edge_t* edge = f.edge[edge_index];
1220 
1221  ccd_pt_face_t* g = edge->faces[0] == &f ? edge->faces[1] : edge->faces[0];
1222  assert(g != nullptr);
1223 
1224  bool is_visible = visible_faces->count(g) > 0;
1225  bool is_hidden = hidden_faces->count(g) > 0;
1226  assert(!(is_visible && is_hidden));
1227 
1228  // First check to see if face `g` has been classifed already.
1229  if (is_visible) {
1230  // Face f is visible (prerequisite), and g has been previously
1231  // marked visible (via a different path); their shared edge should be
1232  // marked internal.
1233  ClassifyInternalEdge(edge, border_edges, internal_edges);
1234  return;
1235  }
1236 
1237  if (is_hidden) {
1238  // Face *has* already been classified as hidden; we just need to classify
1239  // the edge.
1240  ClassifyBorderEdge(edge, border_edges, internal_edges);
1241  return;
1242  }
1243 
1244  // Face `g` has not been classified yet. Try to classify it as visible (i.e.,
1245  // the `query_point` lies outside of this face).
1246  is_visible = isOutsidePolytopeFace(&polytope, g, &query_point);
1247  if (!is_visible) {
1248  // Face `g` is *apparently* not visible from the query point. However, it
1249  // might _still_ be considered visible. The query point could be co-linear
1250  // with `edge` which, by definition means that `g` *is* visible and
1251  // numerical error led to considering it hidden. We can detect this case
1252  // if the triangle formed by edge and query point has zero area.
1253  //
1254  // It may be that the previous designation that `f` was visible was a
1255  // mistake and now face `g` is inheriting that visible status. This is a
1256  // conservative resolution that will prevent us from creating co-planar
1257  // faces (at the cost of a longer border).
1258  is_visible = triangle_area_is_zero(query_point, edge->vertex[0]->v.v,
1259  edge->vertex[1]->v.v);
1260  }
1261 
1262  if (is_visible) {
1263  visible_faces->insert(g);
1264  ClassifyInternalEdge(edge, border_edges, internal_edges);
1265  for (int i = 0; i < 3; ++i) {
1266  if (g->edge[i] != edge) {
1267  // One of the neighbouring face is `f`, so do not need to visit again.
1268  ComputeVisiblePatchRecursive(polytope, *g, i, query_point, border_edges,
1269  visible_faces, hidden_faces,
1270  internal_edges);
1271  }
1272  }
1273  } else {
1274  // No logic could classify the face as visible, so mark it hidden and
1275  // mark the edge as a border edge.
1276  ClassifyBorderEdge(edge, border_edges, internal_edges);
1277  hidden_faces->insert(g);
1278  }
1279 }
1280 
1316  const ccd_pt_t& polytope, ccd_pt_face_t& f,
1317  const ccd_vec3_t& query_point,
1318  std::unordered_set<ccd_pt_edge_t*>* border_edges,
1319  std::unordered_set<ccd_pt_face_t*>* visible_faces,
1320  std::unordered_set<ccd_pt_edge_t*>* internal_edges) {
1321  assert(border_edges);
1322  assert(visible_faces);
1323  assert(internal_edges);
1324  assert(border_edges->empty());
1325  assert(visible_faces->empty());
1326  assert(internal_edges->empty());
1327  assert(isOutsidePolytopeFace(&polytope, &f, &query_point));
1328  std::unordered_set<ccd_pt_face_t*> hidden_faces;
1329  visible_faces->insert(&f);
1330  for (int edge_index = 0; edge_index < 3; ++edge_index) {
1331  ComputeVisiblePatchRecursive(polytope, f, edge_index, query_point,
1332  border_edges, visible_faces, &hidden_faces,
1333  internal_edges);
1334  }
1335 #ifndef NDEBUG
1336  // TODO(SeanCurtis-TRI): Extend the sanity check to include hidden_faces.
1338  polytope, *border_edges, *visible_faces, *internal_edges)) {
1340  "The visible patch failed its sanity check");
1341  }
1342 #endif
1343 }
1344 
1371 static int expandPolytope(ccd_pt_t *polytope, ccd_pt_el_t *el,
1372  const ccd_support_t *newv)
1373 {
1374  // The outline of the algorithm is as follows:
1375  // 1. Compute the visible patch relative to the new vertex (See
1376  // ComputeVisiblePatch() for details).
1377  // 2. Delete the faces and internal edges.
1378  // 3. Build a new face from each border edge and the new vertex.
1379 
1380  // To remove all faces that can be seen from the new vertex, we start with the
1381  // face on which the closest point lives, and then do a depth-first search on
1382  // its neighbouring triangles, until the triangle cannot be seen from the new
1383  // vertex.
1384  // TODO(hongkai.dai@tri.global): it is inefficient to store visible
1385  // faces/edges. A better implementation should remove visible faces and
1386  // internal edges inside ComputeVisiblePatch() function, when traversing the
1387  // faces on the polytope. We focus on the correctness in the first place.
1388  // Later when we make sure that the whole EPA implementation is bug free, we
1389  // will improve the performance.
1390 
1391  ccd_pt_face_t* start_face = NULL;
1392 
1393  if (el->type == CCD_PT_VERTEX) {
1395  "The visible feature is a vertex. This should already have been "
1396  "identified as a touching contact");
1397  }
1398  // Start with the face on which the closest point lives
1399  if (el->type == CCD_PT_FACE) {
1400  start_face = reinterpret_cast<ccd_pt_face_t*>(el);
1401  } else if (el->type == CCD_PT_EDGE) {
1402  // Check the two neighbouring faces of the edge.
1403  ccd_pt_face_t* f[2];
1404  ccdPtEdgeFaces(reinterpret_cast<ccd_pt_edge_t*>(el), &f[0], &f[1]);
1405  if (isOutsidePolytopeFace(polytope, f[0], &newv->v)) {
1406  start_face = f[0];
1407  } else if (isOutsidePolytopeFace(polytope, f[1], &newv->v)) {
1408  start_face = f[1];
1409  } else {
1411  "Both the nearest point and the new vertex are on an edge, thus the "
1412  "nearest distance should be 0. This is touching contact, and should "
1413  "already have been identified");
1414  }
1415  }
1416 
1417  std::unordered_set<ccd_pt_face_t*> visible_faces;
1418  std::unordered_set<ccd_pt_edge_t*> internal_edges;
1419  std::unordered_set<ccd_pt_edge_t*> border_edges;
1420  ComputeVisiblePatch(*polytope, *start_face, newv->v, &border_edges,
1421  &visible_faces, &internal_edges);
1422 
1423  // Now remove all the obsolete faces.
1424  // TODO(hongkai.dai@tri.global): currently we need to loop through each face
1425  // in visible_faces, and then do a linear search in the list pt->faces to
1426  // delete `face`. It would be better if we only loop through the list
1427  // polytope->faces for once. Same for the edges.
1428  for (const auto& f : visible_faces) {
1429  ccdPtDelFace(polytope, f);
1430  }
1431 
1432  // Now remove all the obsolete edges.
1433  for (const auto& e : internal_edges) {
1434  ccdPtDelEdge(polytope, e);
1435  }
1436 
1437  // Note: this does not delete any vertices that were on the interior of the
1438  // deleted patch. There are no longer any faces or edges that reference them.
1439  // Technically, they are still part of the polytope but have no effect (except
1440  // for footprint in memory). It's just as simple to leave them, knowing the
1441  // whole polytope will be destroyed when we're done with the query.
1442 
1443  // A vertex cannot be obsolete, since a vertex is always on the boundary of
1444  // the Minkowski difference A ⊖ B.
1445  // TODO(hongkai.dai@tri.global): as a sanity check, we should make sure that
1446  // all vertices has at least one face/edge invisible from the new vertex
1447  // `newv`.
1448 
1449  // Now add the new vertex.
1450  ccd_pt_vertex_t* new_vertex = ccdPtAddVertex(polytope, newv);
1451 
1452  // Now add the new edges and faces, by connecting the new vertex with vertices
1453  // on border_edges. map_vertex_to_new_edge maps a vertex on the silhouette
1454  // edges to a new edge, with one end being the new vertex, and the other end
1455  // being that vertex on the silhouette edges.
1456  std::unordered_map<ccd_pt_vertex_t*, ccd_pt_edge_t*> map_vertex_to_new_edge;
1457  for (const auto& border_edge : border_edges) {
1458  ccd_pt_edge_t* e[2]; // The two new edges added by connecting new_vertex
1459  // to the two vertices on border_edge.
1460  for (int i = 0; i < 2; ++i) {
1461  auto it = map_vertex_to_new_edge.find(border_edge->vertex[i]);
1462  if (it == map_vertex_to_new_edge.end()) {
1463  // This edge has not been added yet.
1464  e[i] = ccdPtAddEdge(polytope, new_vertex, border_edge->vertex[i]);
1465  map_vertex_to_new_edge.emplace_hint(it, border_edge->vertex[i],
1466  e[i]);
1467  } else {
1468  e[i] = it->second;
1469  }
1470  }
1471  // Now add the face.
1472  ccdPtAddFace(polytope, border_edge, e[0], e[1]);
1473  }
1474 
1475  return 0;
1476 }
1477 
1489 static ccd_vec3_t supportEPADirection(const ccd_pt_t* polytope,
1490  const ccd_pt_el_t* nearest_feature) {
1491  /*
1492  If we denote the nearest point as v, when v is not the origin, then the
1493  support direction is v. If v is the origin, then v should be an interior
1494  point on a face, and the support direction is the normal of that face,
1495  pointing outward from the polytope.
1496  */
1497  ccd_vec3_t dir;
1498  if (ccdIsZero(nearest_feature->dist)) {
1499  // nearest point is the origin.
1500  switch (nearest_feature->type) {
1501  case CCD_PT_VERTEX: {
1503  "The nearest point to the origin is a vertex of the polytope. This "
1504  "should be identified as a touching contact");
1505  break;
1506  }
1507  case CCD_PT_EDGE: {
1508  // When the nearest point is on an edge, the origin must be on that
1509  // edge. The support direction could be in the range between
1510  // edge.faces[0].normal and edge.faces[1].normal, where the face normals
1511  // point outward from the polytope. In this implementation, we
1512  // arbitrarily choose faces[0] normal.
1513  const ccd_pt_edge_t* edge =
1514  reinterpret_cast<const ccd_pt_edge_t*>(nearest_feature);
1515  dir = faceNormalPointingOutward(polytope, edge->faces[0]);
1516  break;
1517  }
1518  case CCD_PT_FACE: {
1519  // If origin is an interior point of a face, then choose the normal of
1520  // that face as the sample direction.
1521  const ccd_pt_face_t* face =
1522  reinterpret_cast<const ccd_pt_face_t*>(nearest_feature);
1523  dir = faceNormalPointingOutward(polytope, face);
1524  break;
1525  }
1526  }
1527  } else {
1528  ccdVec3Copy(&dir, &(nearest_feature->witness));
1529  }
1530  ccdVec3Normalize(&dir);
1531  return dir;
1532 }
1533 
1553 static int nextSupport(const ccd_pt_t* polytope, const void* obj1,
1554  const void* obj2, const ccd_t* ccd,
1555  const ccd_pt_el_t* el, ccd_support_t* out) {
1556  ccd_vec3_t *a, *b, *c;
1557 
1558  if (el->type == CCD_PT_VERTEX) return -1;
1559 
1560  const ccd_vec3_t dir = supportEPADirection(polytope, el);
1561 
1562  __ccdSupport(obj1, obj2, &dir, ccd, out);
1563 
1564  // Compute distance of support point in the support direction, so we can
1565  // determine whether we expanded a polytope surrounding the origin a bit.
1566  const ccd_real_t dist = ccdVec3Dot(&out->v, &dir);
1567 
1568  // el->dist is the squared distance from the feature "el" to the origin..
1569  // dist is an upper bound on the distance from the boundary of the Minkowski
1570  // difference to the origin, and sqrt(el->dist) is a lower bound of that
1571  // distance.
1572  if (dist - std::sqrt(el->dist) < ccd->epa_tolerance) return -1;
1573 
1574  ccd_real_t dist_squared{};
1575  if (el->type == CCD_PT_EDGE) {
1576  // fetch end points of edge
1577  ccdPtEdgeVec3(reinterpret_cast<const ccd_pt_edge_t*>(el), &a, &b);
1578 
1579  // get distance from segment
1580  dist_squared = ccdVec3PointSegmentDist2(&out->v, a, b, NULL);
1581  } else { // el->type == CCD_PT_FACE
1582  // fetch vertices of triangle face
1583  ccdPtFaceVec3(reinterpret_cast<const ccd_pt_face_t*>(el), &a, &b, &c);
1584 
1585  // check if new point can significantly expand polytope
1586  ccd_vec3_t point_projection_on_triangle_unused;
1587  dist_squared = ccdVec3PointTriDist2(&out->v, a, b, c,
1588  &point_projection_on_triangle_unused);
1589  }
1590 
1591  if (std::sqrt(dist_squared) < ccd->epa_tolerance) return -1;
1592 
1593  return 0;
1594 }
1595 
1596 
1597 static int __ccdGJK(const void *obj1, const void *obj2,
1598  const ccd_t *ccd, ccd_simplex_t *simplex)
1599 {
1600  unsigned long iterations;
1601  ccd_vec3_t dir; // direction vector
1602  ccd_support_t last; // last support point
1603  int do_simplex_res;
1604 
1605  // initialize simplex struct
1606  ccdSimplexInit(simplex);
1607 
1608  // get first direction
1609  ccd->first_dir(obj1, obj2, &dir);
1610  // get first support point
1611  __ccdSupport(obj1, obj2, &dir, ccd, &last);
1612  // and add this point to simplex as last one
1613  ccdSimplexAdd(simplex, &last);
1614 
1615  // set up direction vector to as (O - last) which is exactly -last
1616  ccdVec3Copy(&dir, &last.v);
1617  ccdVec3Scale(&dir, -CCD_ONE);
1618 
1619  // start iterations
1620  for (iterations = 0UL; iterations < ccd->max_iterations; ++iterations) {
1621  // obtain support point
1622  __ccdSupport(obj1, obj2, &dir, ccd, &last);
1623 
1624  // check if farthest point in Minkowski difference in direction dir
1625  // isn't somewhere before origin (the test on negative dot product)
1626  // - because if it is, objects are not intersecting at all.
1627  if (ccdVec3Dot(&last.v, &dir) < CCD_ZERO){
1628  return -1; // intersection not found
1629  }
1630 
1631  // add last support vector to simplex
1632  ccdSimplexAdd(simplex, &last);
1633 
1634  // if doSimplex returns 1 if objects intersect, -1 if objects don't
1635  // intersect and 0 if algorithm should continue
1636  do_simplex_res = doSimplex(simplex, &dir);
1637  if (do_simplex_res == 1){
1638  return 0; // intersection found
1639  }else if (do_simplex_res == -1){
1640  return -1; // intersection not found
1641  }
1642 
1643  if (ccdIsZero(ccdVec3Len2(&dir))){
1644  return -1; // intersection not found
1645  }
1646  }
1647 
1648  // intersection wasn't found
1649  return -1;
1650 }
1651 
1671  assert(polytope->nearest_type == CCD_PT_EDGE);
1672 
1673  // We define epsilon to include an additional bit of noise. The goal is to
1674  // pick the smallest epsilon possible. This factor of two proved necessary
1675  // due to unit test behavior on the mac. In the future, as we collect
1676  // more evidence, it may be necessary to increase to more bits. But the need
1677  // should always be demonstrable and not purely theoretical.
1678  constexpr ccd_real_t kEps = 2 * constants<ccd_real_t>::eps();
1679 
1680  // Only verify the feature if the nearest feature is an edge.
1681 
1682  const ccd_pt_edge_t* const nearest_edge =
1683  reinterpret_cast<ccd_pt_edge_t*>(polytope->nearest);
1684  // Find the outward normals on the two neighbouring faces of the edge, if
1685  // the origin is on the "inner" side of these two faces, then we regard the
1686  // origin to be inside the polytope. Note that these face_normals are
1687  // normalized.
1688  std::array<ccd_vec3_t, 2> face_normals;
1689  std::array<double, 2> origin_to_face_distance;
1690 
1691  // We define the plane equation using vertex[0]. If vertex[0] is far away
1692  // from the origin, it can magnify rounding error. We scale epsilon to account
1693  // for this possibility.
1694  const ccd_real_t v0_dist =
1695  std::sqrt(ccdVec3Len2(&nearest_edge->vertex[0]->v.v));
1696  const ccd_real_t plane_threshold =
1697  kEps * std::max(static_cast<ccd_real_t>(1.0), v0_dist);
1698 
1699  for (int i = 0; i < 2; ++i) {
1700  face_normals[i] =
1701  faceNormalPointingOutward(polytope, nearest_edge->faces[i]);
1702  ccdVec3Normalize(&face_normals[i]);
1703  // If the origin o is on the "inner" side of the face, then
1704  // n̂ ⋅ (o - vₑ) ≤ 0 or, with simplification, -n̂ ⋅ vₑ ≤ 0 (since n̂ ⋅ o = 0).
1705  origin_to_face_distance[i] =
1706  -ccdVec3Dot(&face_normals[i], &nearest_edge->vertex[0]->v.v);
1707  // If the origin lies *on* the edge, then it also lies on the two adjacent
1708  // faces. Rather than failing on strictly *positive* signed distance, we
1709  // introduce an epsilon threshold. This usage of epsilon is to account for a
1710  // discrepancy in the signed distance computation. How GJK (and partially
1711  // EPA) compute the signed distance from origin to face may *not* be exactly
1712  // the same as done in this test (i.e. for a given set of vertices, the
1713  // plane equation can be defined various ways. Those ways are
1714  // *mathematically* equivalent but numerically will differ due to rounding).
1715  // We account for those differences by allowing a very small positive signed
1716  // distance to be considered zero. We assume that the GJK/EPA code
1717  // ultimately classifies inside/outside around *zero* and *not* epsilon.
1718  if (origin_to_face_distance[i] > plane_threshold) {
1720  "The origin is outside of the polytope. This should already have "
1721  "been identified as separating.");
1722  }
1723  }
1724 
1725  // We know the reported squared distance to the edge. If that distance is
1726  // functionally zero, then the edge must *truly* be the nearest feature.
1727  // If it isn't, then it must be one of the adjacent faces.
1728  const bool is_edge_closest_feature = nearest_edge->dist < kEps * kEps;
1729 
1730  if (!is_edge_closest_feature) {
1731  // We assume that libccd is not crazily wrong. Although the closest
1732  // feature is not the edge, it is near that edge. Hence we select the
1733  // neighboring face that is closest to the origin.
1734  polytope->nearest_type = CCD_PT_FACE;
1735  // Note origin_to_face_distance is the *signed* distance and it is
1736  // guaranteed to be negative if we are here, hence sense of this
1737  // comparison is reversed.
1738  const int closest_face =
1739  origin_to_face_distance[0] < origin_to_face_distance[1] ? 1 : 0;
1740  polytope->nearest =
1741  reinterpret_cast<ccd_pt_el_t*>(nearest_edge->faces[closest_face]);
1742  // polytope->nearest_dist stores the SQUARED distance.
1743  polytope->nearest_dist = pow(origin_to_face_distance[closest_face], 2);
1744  }
1745 }
1746 
1747 static int __ccdEPA(const void *obj1, const void *obj2,
1748  const ccd_t *ccd,
1749  ccd_simplex_t* simplex,
1750  ccd_pt_t *polytope, ccd_pt_el_t **nearest)
1751 {
1752  ccd_support_t supp; // support point
1753  int ret, size;
1754 
1755 
1756  ret = 0;
1757  *nearest = NULL;
1758 
1759  // transform simplex to polytope - simplex won't be used anymore
1760  size = ccdSimplexSize(simplex);
1761  if (size == 4){
1762  ret = simplexToPolytope4(obj1, obj2, ccd, simplex, polytope, nearest);
1763  } else if (size == 3) {
1764  ret = convert2SimplexToTetrahedron(obj1, obj2, ccd, simplex, polytope,
1765  nearest);
1766  }else{ // size == 2
1767  ret = simplexToPolytope2(obj1, obj2, ccd, simplex, polytope, nearest);
1768  }
1769 
1770 
1771  if (ret == -1){
1772  // touching contact
1773  return 0;
1774  }else if (ret == -2){
1775  // failed memory allocation
1776  return -2;
1777  }
1778 
1779  while (1) {
1780  // get triangle nearest to origin
1781  *nearest = ccdPtNearest(polytope);
1782  if (polytope->nearest_type == CCD_PT_EDGE) {
1783  // When libccd thinks the nearest feature is an edge, that is often
1784  // wrong, hence we validate the nearest feature by ourselves.
1785  // TODO remove this validation step when we can reliably compute the
1786  // nearest feature of a polytope.
1788  *nearest = ccdPtNearest(polytope);
1789  }
1790 
1791  // get next support point
1792  if (nextSupport(polytope, obj1, obj2, ccd, *nearest, &supp) != 0) {
1793  break;
1794  }
1795 
1796  // expand nearest triangle using new point - supp
1797  if (expandPolytope(polytope, *nearest, &supp) != 0) return -2;
1798  }
1799 
1800  return 0;
1801 }
1802 
1805 static void extractObjectPointsFromPoint(ccd_support_t *q, ccd_vec3_t *p1,
1806  ccd_vec3_t *p2) {
1807  // TODO(SeanCurtis-TRI): Determine if I should be demanding that p1 and p2
1808  // are defined.
1809  // Closest points are the ones stored in the simplex
1810  if (p1) *p1 = q->v1;
1811  if (p2) *p2 = q->v2;
1812 }
1813 
1819  ccd_vec3_t *p1, ccd_vec3_t *p2,
1820  ccd_vec3_t *p) {
1821  // Closest points lie on the segment defined by the points in the simplex
1822  // Let the segment be defined by points A and B. We can write p as
1823  //
1824  // p = A + s*AB, 0 <= s <= 1
1825  // p - A = s*AB
1826  ccd_vec3_t AB;
1827  ccdVec3Sub2(&AB, &(b->v), &(a->v));
1828 
1829  // This defines three equations, but we only need one. Taking the i-th
1830  // component gives
1831  //
1832  // p_i - A_i = s*AB_i.
1833  //
1834  // Thus, s is given by
1835  //
1836  // s = (p_i - A_i)/AB_i.
1837  //
1838  // To avoid dividing by an AB_i ≪ 1, we choose i such that |AB_i| is
1839  // maximized
1840  ccd_real_t abs_AB_x{std::abs(ccdVec3X(&AB))};
1841  ccd_real_t abs_AB_y{std::abs(ccdVec3Y(&AB))};
1842  ccd_real_t abs_AB_z{std::abs(ccdVec3Z(&AB))};
1843 
1844  ccd_real_t A_i, AB_i, p_i;
1845  if (abs_AB_x >= abs_AB_y && abs_AB_x >= abs_AB_z) {
1846  A_i = ccdVec3X(&(a->v));
1847  AB_i = ccdVec3X(&AB);
1848  p_i = ccdVec3X(p);
1849  } else if (abs_AB_y >= abs_AB_z) {
1850  A_i = ccdVec3Y(&(a->v));
1851  AB_i = ccdVec3Y(&AB);
1852  p_i = ccdVec3Y(p);
1853  } else {
1854  A_i = ccdVec3Z(&(a->v));
1855  AB_i = ccdVec3Z(&AB);
1856  p_i = ccdVec3Z(p);
1857  }
1858 
1859  if (std::abs(AB_i) < constants<ccd_real_t>::eps()) {
1860  // Points are coincident; treat as a single point.
1861  extractObjectPointsFromPoint(a, p1, p2);
1862  return;
1863  }
1864 
1865  auto calc_p = [](ccd_vec3_t *p_a, ccd_vec3_t *p_b, ccd_vec3_t *p,
1866  ccd_real_t s) {
1867  ccd_vec3_t sAB;
1868  ccdVec3Sub2(&sAB, p_b, p_a);
1869  ccdVec3Scale(&sAB, s);
1870  ccdVec3Copy(p, p_a);
1871  ccdVec3Add(p, &sAB);
1872  };
1873 
1874  // TODO(SeanCurtis-TRI): If p1 or p2 is null, there seems little point in
1875  // calling this method. It seems that both of these being non-null should be
1876  // a *requirement*. Determine that this is the case and do so.
1877  ccd_real_t s = (p_i - A_i) / AB_i;
1878 
1879  if (p1) {
1880  // p1 = A1 + s*A1B1
1881  calc_p(&(a->v1), &(b->v1), p1, s);
1882  }
1883  if (p2) {
1884  // p2 = A2 + s*A2B2
1885  calc_p(&(a->v2), &(b->v2), p2, s);
1886  }
1887 }
1888 
1894 static void extractClosestPoints(ccd_simplex_t *simplex, ccd_vec3_t *p1,
1895  ccd_vec3_t *p2, ccd_vec3_t *p) {
1896  const int simplex_size = ccdSimplexSize(simplex);
1897  assert(simplex_size <= 3);
1898  if (simplex_size == 1)
1899  {
1900  extractObjectPointsFromPoint(&simplex->ps[0], p1, p2);
1901  }
1902  else if (simplex_size == 2)
1903  {
1904  extractObjectPointsFromSegment(&simplex->ps[0], &simplex->ps[1], p1, p2, p);
1905  }
1906  else // simplex_size == 3
1907  {
1908  if (triangle_area_is_zero(simplex->ps[0].v, simplex->ps[1].v,
1909  simplex->ps[2].v)) {
1910  // The triangle is degenerate; compute the nearest point to a line
1911  // segment. The segment is defined by the most-distant vertex pair.
1912  int a_index, b_index;
1913  ccd_vec3_t AB, AC, BC;
1914  ccdVec3Sub2(&AB, &(simplex->ps[1].v), &(simplex->ps[0].v));
1915  ccdVec3Sub2(&AC, &(simplex->ps[2].v), &(simplex->ps[0].v));
1916  ccdVec3Sub2(&BC, &(simplex->ps[2].v), &(simplex->ps[1].v));
1917  ccd_real_t AB_len2 = ccdVec3Len2(&AB);
1918  ccd_real_t AC_len2 = ccdVec3Len2(&AC);
1919  ccd_real_t BC_len2 = ccdVec3Len2(&BC);
1920  if (AB_len2 >= AC_len2 && AB_len2 >= BC_len2) {
1921  a_index = 0;
1922  b_index = 1;
1923  } else if (AC_len2 >= AB_len2 && AC_len2 >= BC_len2) {
1924  a_index = 0;
1925  b_index = 2;
1926  } else {
1927  a_index = 1;
1928  b_index = 2;
1929  }
1930  extractObjectPointsFromSegment(&simplex->ps[a_index],
1931  &simplex->ps[b_index], p1, p2, p);
1932  return;
1933  }
1934 
1935  // Compute the barycentric coordinates of point p in triangle ABC.
1936  //
1937  // A
1938  // ╱╲ p = αA + βB + γC
1939  // ╱ |╲
1940  // ╱ | ╲ α = 1 - β - γ
1941  // ╱ p | ╲ β = AREA(pAC) / AREA(ABC)
1942  // ╱ / \ ╲ γ = AREA(pAB) / AREA(ABC)
1943  // ╱__/_____\_╲
1944  // B C AREA(XYZ) = |r_XY × r_XZ| / 2
1945  //
1946  // Rewrite coordinates in terms of cross products.
1947  //
1948  // β = AREA(pAC) / AREA(ABC) = |r_Ap × r_AC| / |r_AB × r_AC|
1949  // γ = AREA(pAB) / AREA(ABC) = |r_AB × r_Ap| / |r_AB × r_AC|
1950  //
1951  // NOTE: There are multiple options for the cross products, these have been
1952  // selected to re-use as many symbols as possible.
1953  //
1954  // Solving for β and γ:
1955  //
1956  // β = |r_Ap × r_AC| / |r_AB × r_AC|
1957  // β = |r_Ap × r_AC| / |n| n ≙ r_AB × r_AC, n̂ = n / |n|
1958  // β = n̂·(r_Ap × r_AC) / n̂·n This step arises from the fact
1959  // that (r_Ap × r_AC) and n point
1960  // in the same direction. It
1961  // allows us to take a single sqrt
1962  // instead of three.
1963  // β = (n/|n|)·(r_Ap × r_AC) / (n/|n|)·n
1964  // β = n·(r_Ap × r_AC) / n·n
1965  // β = n·(r_Ap × r_AC) / |n|²
1966  //
1967  // A similar process to solve for gamma
1968  // γ = n·(r_AB × r_Ap) / |n|²
1969 
1970  // Compute n and |n|².
1971  ccd_vec3_t r_AB, r_AC, n;
1972  ccdVec3Sub2(&r_AB, &(simplex->ps[1].v), &(simplex->ps[0].v));
1973  ccdVec3Sub2(&r_AC, &(simplex->ps[2].v), &(simplex->ps[0].v));
1974  ccdVec3Cross(&n, &r_AB, &r_AC);
1975  ccd_real_t norm_squared_n{ccdVec3Len2(&n)};
1976 
1977  // Compute r_Ap.
1978  ccd_vec3_t r_Ap;
1979  ccdVec3Sub2(&r_Ap, p, &(simplex->ps[0].v));
1980 
1981  // Compute the cross products in the numerators.
1982  ccd_vec3_t r_Ap_cross_r_AC, r_AB_cross_r_Ap;
1983  ccdVec3Cross(&r_Ap_cross_r_AC, &r_Ap, &r_AC); // r_Ap × r_AC
1984  ccdVec3Cross(&r_AB_cross_r_Ap, &r_AB, &r_Ap); // r_AB × r_Ap
1985 
1986  // Compute beta and gamma.
1987  ccd_real_t beta{ccdVec3Dot(&n, &r_Ap_cross_r_AC) / norm_squared_n};
1988  ccd_real_t gamma{ccdVec3Dot(&n, &r_AB_cross_r_Ap) / norm_squared_n};
1989 
1990  // Evaluate barycentric interpolation (with the locally defined barycentric
1991  // coordinates).
1992  auto interpolate = [&beta, &gamma](const ccd_vec3_t& r_WA,
1993  const ccd_vec3_t& r_WB,
1994  const ccd_vec3_t& r_WC,
1995  ccd_vec3_t* r_WP) {
1996  // r_WP = r_WA + β * r_AB + γ * r_AC
1997  ccdVec3Copy(r_WP, &r_WA);
1998 
1999  ccd_vec3_t beta_r_AB;
2000  ccdVec3Sub2(&beta_r_AB, &r_WB, &r_WA);
2001  ccdVec3Scale(&beta_r_AB, beta);
2002  ccdVec3Add(r_WP, &beta_r_AB);
2003 
2004  ccd_vec3_t gamma_r_AC;
2005  ccdVec3Sub2(&gamma_r_AC, &r_WC, &r_WA);
2006  ccdVec3Scale(&gamma_r_AC, gamma);
2007  ccdVec3Add(r_WP, &gamma_r_AC);
2008  };
2009 
2010  if (p1) {
2011  interpolate(simplex->ps[0].v1, simplex->ps[1].v1, simplex->ps[2].v1, p1);
2012  }
2013 
2014  if (p2) {
2015  interpolate(simplex->ps[0].v2, simplex->ps[1].v2, simplex->ps[2].v2, p2);
2016  }
2017  }
2018 }
2019 
2020 // Computes the distance between two non-penetrating convex objects, `obj1` and
2021 // `obj2` based on the warm-started witness simplex, returning the distance and
2022 // nearest points on each object.
2023 // @param obj1 The first convex object.
2024 // @param obj2 The second convex object.
2025 // @param ccd The libccd configuration.
2026 // @param simplex A witness to the objects' separation generated by the GJK
2027 // algorithm. NOTE: the simplex is not necessarily sufficiently refined to
2028 // report the actual distance and may be further refined in this method.
2029 // @param p1 If the objects are non-penetrating, the point on the surface of
2030 // obj1 closest to obj2 (expressed in the world frame).
2031 // @param p2 If the objects are non-penetrating, the point on the surface of
2032 // obj2 closest to obj1 (expressed in the world frame).
2033 // @returns The minimum distance between the two objects. If they are
2034 // penetrating, -1 is returned.
2035 static inline ccd_real_t _ccdDist(const void *obj1, const void *obj2,
2036  const ccd_t *ccd,
2037  ccd_simplex_t* simplex,
2038  ccd_vec3_t* p1, ccd_vec3_t* p2)
2039 {
2040  ccd_real_t last_dist = CCD_REAL_MAX;
2041 
2042  for (unsigned long iterations = 0UL; iterations < ccd->max_iterations;
2043  ++iterations) {
2044  ccd_vec3_t closest_p; // The point on the simplex that is closest to the
2045  // origin.
2046  ccd_real_t dist;
2047  // get a next direction vector
2048  // we are trying to find out a point on the minkowski difference
2049  // that is nearest to the origin, so we obtain a point on the
2050  // simplex that is nearest and try to exapand the simplex towards
2051  // the origin
2052  if (ccdSimplexSize(simplex) == 1)
2053  {
2054  ccdVec3Copy(&closest_p, &ccdSimplexPoint(simplex, 0)->v);
2055  dist = ccdVec3Len2(&ccdSimplexPoint(simplex, 0)->v);
2056  dist = CCD_SQRT(dist);
2057  }
2058  else if (ccdSimplexSize(simplex) == 2)
2059  {
2060  dist = ccdVec3PointSegmentDist2(ccd_vec3_origin,
2061  &ccdSimplexPoint(simplex, 0)->v,
2062  &ccdSimplexPoint(simplex, 1)->v,
2063  &closest_p);
2064  dist = CCD_SQRT(dist);
2065  }
2066  else if (ccdSimplexSize(simplex) == 3)
2067  {
2068  dist = ccdVec3PointTriDist2(ccd_vec3_origin,
2069  &ccdSimplexPoint(simplex, 0)->v,
2070  &ccdSimplexPoint(simplex, 1)->v,
2071  &ccdSimplexPoint(simplex, 2)->v,
2072  &closest_p);
2073  dist = CCD_SQRT(dist);
2074  }
2075  else
2076  { // ccdSimplexSize(&simplex) == 4
2077  dist = simplexReduceToTriangle(simplex, last_dist, &closest_p);
2078  }
2079 
2080  // check whether we improved for at least a minimum tolerance
2081  if ((last_dist - dist) < ccd->dist_tolerance)
2082  {
2083  extractClosestPoints(simplex, p1, p2, &closest_p);
2084  return dist;
2085  }
2086 
2087  // point direction towards the origin
2088  ccd_vec3_t dir; // direction vector
2089  ccdVec3Copy(&dir, &closest_p);
2090  ccdVec3Scale(&dir, -CCD_ONE);
2091  ccdVec3Normalize(&dir);
2092 
2093  // find out support point
2094  ccd_support_t last; // last support point
2095  __ccdSupport(obj1, obj2, &dir, ccd, &last);
2096 
2097  // record last distance
2098  last_dist = dist;
2099 
2100  // check whether we improved for at least a minimum tolerance
2101  // this is here probably only for a degenerate cases when we got a
2102  // point that is already in the simplex
2103  dist = ccdVec3Len2(&last.v);
2104  dist = CCD_SQRT(dist);
2105  if (CCD_FABS(last_dist - dist) < ccd->dist_tolerance)
2106  {
2107  extractClosestPoints(simplex, p1, p2, &closest_p);
2108  return last_dist;
2109  }
2110 
2111  // add a point to simplex
2112  ccdSimplexAdd(simplex, &last);
2113  }
2114 
2115  return -CCD_REAL(1.);
2116 }
2117 
2131 static int penEPAPosClosest(const ccd_pt_el_t* nearest, ccd_vec3_t* p1,
2132  ccd_vec3_t* p2) {
2133  // We reconstruct the simplex on which the nearest point lives, and then
2134  // compute the deepest penetration point on each geometric objects. Note that
2135  // the reconstructed simplex has size up to 3 (at most 3 vertices).
2136  if (nearest->type == CCD_PT_VERTEX) {
2137  ccd_pt_vertex_t* v = (ccd_pt_vertex_t*)nearest;
2138  ccdVec3Copy(p1, &v->v.v1);
2139  ccdVec3Copy(p2, &v->v.v2);
2140  return 0;
2141  } else {
2142  ccd_simplex_t s;
2143  ccdSimplexInit(&s);
2144  if (nearest->type == CCD_PT_EDGE) {
2145  const ccd_pt_edge_t* e = reinterpret_cast<const ccd_pt_edge_t*>(nearest);
2146  ccdSimplexAdd(&s, &(e->vertex[0]->v));
2147  ccdSimplexAdd(&s, &(e->vertex[1]->v));
2148  } else if (nearest->type == CCD_PT_FACE) {
2149  const ccd_pt_face_t* f = reinterpret_cast<const ccd_pt_face_t*>(nearest);
2150  // The face triangle has three edges, each edge consists of two end
2151  // points, so there are 6 end points in total, each vertex of the triangle
2152  // appears twice among the 6 end points. We need to choose the three
2153  // distinctive vertices out of these 6 end points.
2154  // First we pick edge0, the two end points of edge0 are distinct.
2155  ccdSimplexAdd(&s, &(f->edge[0]->vertex[0]->v));
2156  ccdSimplexAdd(&s, &(f->edge[0]->vertex[1]->v));
2157  // Next we pick edge1, one of the two end points on edge1 is distinct from
2158  // the end points in edge0, we will add this distinct vertex to the
2159  // simplex.
2160  for (int i = 0; i < 2; ++i) {
2161  ccd_pt_vertex_t* third_vertex = f->edge[1]->vertex[i];
2162  if (third_vertex != f->edge[0]->vertex[0] &&
2163  third_vertex != f->edge[0]->vertex[1]) {
2164  ccdSimplexAdd(&s, &(third_vertex->v));
2165  break;
2166  }
2167  }
2168  } else {
2169  throw std::logic_error(
2170  "FCL penEPAPosClosest(): Unsupported feature type. The closest point "
2171  "should be either a vertex, on an edge, or on a face.");
2172  }
2173  // Now compute the closest point in the simplex.
2174  // TODO(hongkai.dai@tri.global): we do not need to compute the closest point
2175  // on the simplex, as that is already given in `nearest`. We only need to
2176  // extract the deepest penetration points on each geometric object.
2177  // Sean.Curtis@tri.global and I will refactor this code in the future, to
2178  // avoid calling extractClosestPoints.
2179  ccd_vec3_t p;
2180  ccdVec3Copy(&p, &(nearest->witness));
2181  extractClosestPoints(&s, p1, p2, &p);
2182  return 0;
2183  }
2184 }
2185 
2186 static inline ccd_real_t ccdGJKSignedDist(const void* obj1, const void* obj2,
2187  const ccd_t* ccd, ccd_vec3_t* p1,
2188  ccd_vec3_t* p2)
2189 {
2190  ccd_simplex_t simplex;
2191 
2192  if (__ccdGJK(obj1, obj2, ccd, &simplex) == 0) // in collision, then using the EPA
2193  {
2194  ccd_pt_t polytope;
2195  ccd_pt_el_t *nearest;
2196  ccd_real_t depth;
2197 
2198  ccdPtInit(&polytope);
2199  int ret = __ccdEPA(obj1, obj2, ccd, &simplex, &polytope, &nearest);
2200  if (ret == 0 && nearest)
2201  {
2202  depth = -CCD_SQRT(nearest->dist);
2203 
2204  ccd_vec3_t pos1, pos2;
2205  penEPAPosClosest(nearest, &pos1, &pos2);
2206 
2207  if (p1) *p1 = pos1;
2208  if (p2) *p2 = pos2;
2209 
2210  //ccd_vec3_t dir; // direction vector
2211  //ccdVec3Copy(&dir, &nearest->witness);
2212  //std::cout << dir.v[0] << " " << dir.v[1] << " " << dir.v[2] << std::endl;
2213  //ccd_support_t last;
2214  //__ccdSupport(obj1, obj2, &dir, ccd, &last);
2215 
2216  //if (p1) *p1 = last.v1;
2217  //if (p2) *p2 = last.v2;
2218  }
2219  else
2220  {
2221  depth = -CCD_ONE;
2222  }
2223 
2224  ccdPtDestroy(&polytope);
2225 
2226  return depth;
2227  }
2228  else // not in collision
2229  {
2230  return _ccdDist(obj1, obj2, ccd, &simplex, p1, p2);
2231  }
2232 }
2233 
2234 
2235 // Computes the distance between two non-penetrating convex objects, `obj1` and
2236 // `obj2`, returning the distance and
2237 // nearest points on each object.
2238 // @param obj1 The first convex object.
2239 // @param obj2 The second convex object.
2240 // @param ccd The libccd configuration.
2241 // @param p1 If the objects are non-penetrating, the point on the surface of
2242 // obj1 closest to obj2 (expressed in the world frame).
2243 // @param p2 If the objects are non-penetrating, the point on the surface of
2244 // obj2 closest to obj1 (expressed in the world frame).
2245 // @returns The minimum distance between the two objects. If they are
2246 // penetrating, -1 is returned.
2247 // @note Unlike _ccdDist function, this function does not need a warm-started
2248 // simplex as the input argument.
2249 static inline ccd_real_t ccdGJKDist2(const void *obj1, const void *obj2,
2250  const ccd_t *ccd, ccd_vec3_t* p1,
2251  ccd_vec3_t* p2)
2252 {
2253  ccd_simplex_t simplex;
2254  // first find an intersection
2255  if (__ccdGJK(obj1, obj2, ccd, &simplex) == 0)
2256  return -CCD_ONE;
2257 
2258  return _ccdDist(obj1, obj2, ccd, &simplex, p1, p2);
2259 }
2260 
2261 } // namespace libccd_extension
2262 
2264 template <typename S>
2265 static void shapeToGJK(const ShapeBase<S>& s, const Transform3<S>& tf,
2266  ccd_obj_t* o)
2267 {
2268  FCL_UNUSED(s);
2269 
2270  const Quaternion<S> q(tf.linear());
2271  const Vector3<S>& T = tf.translation();
2272  ccdVec3Set(&o->pos, T[0], T[1], T[2]);
2273  ccdQuatSet(&o->rot, q.x(), q.y(), q.z(), q.w());
2274  ccdQuatInvert2(&o->rot_inv, &o->rot);
2275 }
2276 
2277 template <typename S>
2278 static void boxToGJK(const Box<S>& s, const Transform3<S>& tf, ccd_box_t* box)
2279 {
2280  shapeToGJK(s, tf, box);
2281  box->dim[0] = s.side[0] / 2.0;
2282  box->dim[1] = s.side[1] / 2.0;
2283  box->dim[2] = s.side[2] / 2.0;
2284 }
2285 
2286 template <typename S>
2287 static void capToGJK(const Capsule<S>& s, const Transform3<S>& tf,
2288  ccd_cap_t* cap)
2289 {
2290  shapeToGJK(s, tf, cap);
2291  cap->radius = s.radius;
2292  cap->height = s.lz / 2;
2293 }
2294 
2295 template <typename S>
2296 static void cylToGJK(const Cylinder<S>& s, const Transform3<S>& tf,
2297  ccd_cyl_t* cyl)
2298 {
2299  shapeToGJK(s, tf, cyl);
2300  cyl->radius = s.radius;
2301  cyl->height = s.lz / 2;
2302 }
2303 
2304 template <typename S>
2305 static void coneToGJK(const Cone<S>& s, const Transform3<S>& tf,
2306  ccd_cone_t* cone)
2307 {
2308  shapeToGJK(s, tf, cone);
2309  cone->radius = s.radius;
2310  cone->height = s.lz / 2;
2311 }
2312 
2313 template <typename S>
2314 static void sphereToGJK(const Sphere<S>& s, const Transform3<S>& tf,
2315  ccd_sphere_t* sph)
2316 {
2317  shapeToGJK(s, tf, sph);
2318  sph->radius = s.radius;
2319 }
2320 
2321 template <typename S>
2322 static void ellipsoidToGJK(const Ellipsoid<S>& s, const Transform3<S>& tf,
2323  ccd_ellipsoid_t* ellipsoid)
2324 {
2325  shapeToGJK(s, tf, ellipsoid);
2326  ellipsoid->radii[0] = s.radii[0];
2327  ellipsoid->radii[1] = s.radii[1];
2328  ellipsoid->radii[2] = s.radii[2];
2329 }
2330 
2331 template <typename S>
2332 static void convexToGJK(const Convex<S>& s, const Transform3<S>& tf,
2333  ccd_convex_t<S>* conv)
2334 {
2335  shapeToGJK(s, tf, conv);
2336  conv->convex = &s;
2337 }
2338 
2340 static inline void supportBox(const void* obj, const ccd_vec3_t* dir_,
2341  ccd_vec3_t* v)
2342 {
2343  // Use a customized sign function, so that the support of the box always
2344  // appears in one of the box vertices.
2345  // Picking support vertices on the interior of faces/edges can lead to
2346  // degenerate triangles in the EPA algorithm and are no more correct than just
2347  // picking box corners.
2348  auto sign = [](ccd_real_t x) -> ccd_real_t {
2349  return x >= 0 ? ccd_real_t(1.0) : ccd_real_t(-1.0);
2350  };
2351  const ccd_box_t* o = static_cast<const ccd_box_t*>(obj);
2352  ccd_vec3_t dir;
2353  ccdVec3Copy(&dir, dir_);
2354  ccdQuatRotVec(&dir, &o->rot_inv);
2355  ccdVec3Set(v, sign(ccdVec3X(&dir)) * o->dim[0],
2356  sign(ccdVec3Y(&dir)) * o->dim[1],
2357  sign(ccdVec3Z(&dir)) * o->dim[2]);
2358  ccdQuatRotVec(v, &o->rot);
2359  ccdVec3Add(v, &o->pos);
2360 }
2361 
2362 static inline void supportCap(const void* obj, const ccd_vec3_t* dir_,
2363  ccd_vec3_t* v)
2364 {
2365  const ccd_cap_t* o = static_cast<const ccd_cap_t*>(obj);
2366  ccd_vec3_t dir, pos1, pos2;
2367 
2368  ccdVec3Copy(&dir, dir_);
2369  ccdQuatRotVec(&dir, &o->rot_inv);
2370 
2371  ccdVec3Set(&pos1, CCD_ZERO, CCD_ZERO, o->height);
2372  ccdVec3Set(&pos2, CCD_ZERO, CCD_ZERO, -o->height);
2373 
2374  ccdVec3Copy(v, &dir);
2375  ccdVec3Normalize (v);
2376  ccdVec3Scale(v, o->radius);
2377  ccdVec3Add(&pos1, v);
2378  ccdVec3Add(&pos2, v);
2379 
2380  if (ccdVec3Z (&dir) > 0)
2381  ccdVec3Copy(v, &pos1);
2382  else
2383  ccdVec3Copy(v, &pos2);
2384 
2385  // transform support vertex
2386  ccdQuatRotVec(v, &o->rot);
2387  ccdVec3Add(v, &o->pos);
2388 }
2389 
2390 static inline void supportCyl(const void* obj, const ccd_vec3_t* dir_,
2391  ccd_vec3_t* v)
2392 {
2393  const ccd_cyl_t* cyl = static_cast<const ccd_cyl_t*>(obj);
2394  ccd_vec3_t dir;
2395  double zdist, rad;
2396 
2397  ccdVec3Copy(&dir, dir_);
2398  ccdQuatRotVec(&dir, &cyl->rot_inv);
2399 
2400  zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1];
2401  zdist = sqrt(zdist);
2402  if (ccdIsZero(zdist))
2403  ccdVec3Set(v, 0., 0., ccdSign(ccdVec3Z(&dir)) * cyl->height);
2404  else
2405  {
2406  rad = cyl->radius / zdist;
2407 
2408  ccdVec3Set(v, rad * ccdVec3X(&dir),
2409  rad * ccdVec3Y(&dir),
2410  ccdSign(ccdVec3Z(&dir)) * cyl->height);
2411  }
2412 
2413  // transform support vertex
2414  ccdQuatRotVec(v, &cyl->rot);
2415  ccdVec3Add(v, &cyl->pos);
2416 }
2417 
2418 static inline void supportCone(const void* obj, const ccd_vec3_t* dir_,
2419  ccd_vec3_t* v)
2420 {
2421  const ccd_cone_t* cone = static_cast<const ccd_cone_t*>(obj);
2422  ccd_vec3_t dir;
2423 
2424  ccdVec3Copy(&dir, dir_);
2425  ccdQuatRotVec(&dir, &cone->rot_inv);
2426 
2427  double zdist, len, rad;
2428  zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1];
2429  len = zdist + dir.v[2] * dir.v[2];
2430  zdist = sqrt(zdist);
2431  len = sqrt(len);
2432 
2433  double sin_a = cone->radius / sqrt(cone->radius * cone->radius + 4 * cone->height * cone->height);
2434 
2435  if (dir.v[2] > len * sin_a)
2436  ccdVec3Set(v, 0., 0., cone->height);
2437  else if (zdist > 0)
2438  {
2439  rad = cone->radius / zdist;
2440  ccdVec3Set(v, rad * ccdVec3X(&dir), rad * ccdVec3Y(&dir), -cone->height);
2441  }
2442  else
2443  ccdVec3Set(v, 0, 0, -cone->height);
2444 
2445  // transform support vertex
2446  ccdQuatRotVec(v, &cone->rot);
2447  ccdVec3Add(v, &cone->pos);
2448 }
2449 
2450 static inline void supportSphere(const void* obj, const ccd_vec3_t* dir_,
2451  ccd_vec3_t* v)
2452 {
2453  const ccd_sphere_t* s = static_cast<const ccd_sphere_t*>(obj);
2454  ccd_vec3_t dir;
2455 
2456  ccdVec3Copy(&dir, dir_);
2457  ccdQuatRotVec(&dir, &s->rot_inv);
2458 
2459  ccdVec3Copy(v, &dir);
2460  ccdVec3Scale(v, s->radius);
2461  ccdVec3Scale(v, CCD_ONE / CCD_SQRT(ccdVec3Len2(&dir)));
2462 
2463  // transform support vertex
2464  ccdQuatRotVec(v, &s->rot);
2465  ccdVec3Add(v, &s->pos);
2466 }
2467 
2468 static inline void supportEllipsoid(const void* obj, const ccd_vec3_t* dir_,
2469  ccd_vec3_t* v)
2470 {
2471  const ccd_ellipsoid_t* s = static_cast<const ccd_ellipsoid_t*>(obj);
2472  ccd_vec3_t dir;
2473 
2474  ccdVec3Copy(&dir, dir_);
2475  ccdQuatRotVec(&dir, &s->rot_inv);
2476 
2477  ccd_vec3_t abc2;
2478  abc2.v[0] = s->radii[0] * s->radii[0];
2479  abc2.v[1] = s->radii[1] * s->radii[1];
2480  abc2.v[2] = s->radii[2] * s->radii[2];
2481 
2482  v->v[0] = abc2.v[0] * dir.v[0];
2483  v->v[1] = abc2.v[1] * dir.v[1];
2484  v->v[2] = abc2.v[2] * dir.v[2];
2485 
2486  ccdVec3Scale(v, CCD_ONE / CCD_SQRT(ccdVec3Dot(v, &dir)));
2487 
2488  // transform support vertex
2489  ccdQuatRotVec(v, &s->rot);
2490  ccdVec3Add(v, &s->pos);
2491 }
2492 
2493 template <typename S>
2494 static void supportConvex(const void* obj, const ccd_vec3_t* dir_,
2495  ccd_vec3_t* v)
2496 {
2497  const auto* c = (const ccd_convex_t<S>*)obj;
2498 
2499  // Transform the support direction vector from the query frame Q to the
2500  // convex mesh's local frame C. I.e., dir_Q -> dir_C.
2501  ccd_vec3_t dir;
2502  ccdVec3Copy(&dir, dir_);
2503  ccdQuatRotVec(&dir, &c->rot_inv);
2504  // Note: The scalar type ccd_real_t is fixed w.r.t. S. That means if S is
2505  // float and ccd_real_t is double, this conversion requires narrowing. To
2506  // avoid warning/errors about implicit narrowing, we explicitly convert.
2507  Vector3<S> dir_C{S(dir.v[0]), S(dir.v[1]), S(dir.v[2])};
2508 
2509  // The extremal point E measured and expressed in Frame C.
2510  const Vector3<S>& p_CE = c->convex->findExtremeVertex(dir_C);
2511 
2512  // Now measure and express E in the original query frame Q: p_CE -> p_QE.
2513  v->v[0] = p_CE(0);
2514  v->v[1] = p_CE(1);
2515  v->v[2] = p_CE(2);
2516  ccdQuatRotVec(v, &c->rot);
2517  ccdVec3Add(v, &c->pos);
2518 }
2519 
2520 static void supportTriangle(const void* obj, const ccd_vec3_t* dir_,
2521  ccd_vec3_t* v)
2522 {
2523  const ccd_triangle_t* tri = static_cast<const ccd_triangle_t*>(obj);
2524  ccd_vec3_t dir, p;
2525  ccd_real_t maxdot, dot;
2526  int i;
2527 
2528  ccdVec3Copy(&dir, dir_);
2529  ccdQuatRotVec(&dir, &tri->rot_inv);
2530 
2531  maxdot = -CCD_REAL_MAX;
2532 
2533  for (i = 0; i < 3; ++i)
2534  {
2535  ccdVec3Set(&p, tri->p[i].v[0] - tri->c.v[0], tri->p[i].v[1] - tri->c.v[1],
2536  tri->p[i].v[2] - tri->c.v[2]);
2537  dot = ccdVec3Dot(&dir, &p);
2538  if (dot > maxdot)
2539  {
2540  ccdVec3Copy(v, &tri->p[i]);
2541  maxdot = dot;
2542  }
2543  }
2544 
2545  // transform support vertex
2546  ccdQuatRotVec(v, &tri->rot);
2547  ccdVec3Add(v, &tri->pos);
2548 }
2549 
2550 static inline void centerShape(const void* obj, ccd_vec3_t* c)
2551 {
2552  const ccd_obj_t *o = static_cast<const ccd_obj_t*>(obj);
2553  ccdVec3Copy(c, &o->pos);
2554 }
2555 
2556 template <typename S>
2557 static void centerConvex(const void* obj, ccd_vec3_t* c)
2558 {
2559  const auto *o = static_cast<const ccd_convex_t<S>*>(obj);
2560  const Vector3<S>& p = o->convex->getInteriorPoint();
2561  ccdVec3Set(c, p[0], p[1], p[2]);
2562  ccdQuatRotVec(c, &o->rot);
2563  ccdVec3Add(c, &o->pos);
2564 }
2565 
2566 static void centerTriangle(const void* obj, ccd_vec3_t* c)
2567 {
2568  const ccd_triangle_t *o = static_cast<const ccd_triangle_t*>(obj);
2569  ccdVec3Copy(c, &o->c);
2570  ccdQuatRotVec(c, &o->rot);
2571  ccdVec3Add(c, &o->pos);
2572 }
2573 
2574 template <typename S>
2575 bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
2576  void* obj2, ccd_support_fn supp2, ccd_center_fn cen2,
2577  unsigned int max_iterations, S tolerance,
2578  Vector3<S>* contact_points, S* penetration_depth,
2579  Vector3<S>* normal)
2580 {
2581  ccd_t ccd;
2582  int res;
2583  ccd_real_t depth;
2584  ccd_vec3_t dir, pos;
2585 
2586 
2587  CCD_INIT(&ccd);
2588  ccd.support1 = supp1;
2589  ccd.support2 = supp2;
2590  ccd.center1 = cen1;
2591  ccd.center2 = cen2;
2592  ccd.max_iterations = max_iterations;
2593  ccd.mpr_tolerance = tolerance;
2594 
2595  if (!contact_points)
2596  {
2597  return ccdMPRIntersect(obj1, obj2, &ccd);
2598  }
2599 
2600 
2603  res = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos);
2604  if (res == 0)
2605  {
2606  *contact_points << ccdVec3X(&pos), ccdVec3Y(&pos), ccdVec3Z(&pos);
2607  *penetration_depth = depth;
2608  *normal << ccdVec3X(&dir), ccdVec3Y(&dir), ccdVec3Z(&dir);
2609 
2610  return true;
2611  }
2612 
2613  return false;
2614 }
2615 
2616 // For two geometric objects, computes the distance between the two objects and
2617 // returns the closest points. The return argument is the distance when the two
2618 // objects are not colliding, thus a non-negative number; it is a negative
2619 // number when the object is colliding, though the meaning of that negative
2620 // number depends on the implementation.
2621 using DistanceFn = std::function<ccd_real_t (
2622  const void*, const void*, const ccd_t*, ccd_vec3_t*, ccd_vec3_t*)>;
2623 
2646 template <typename S>
2647 bool GJKDistanceImpl(void* obj1, ccd_support_fn supp1, void* obj2,
2648  ccd_support_fn supp2, unsigned int max_iterations,
2649  S tolerance, detail::DistanceFn distance_func, S* res,
2650  Vector3<S>* p1, Vector3<S>* p2) {
2651  ccd_t ccd;
2652  ccd_real_t dist;
2653  CCD_INIT(&ccd);
2654  ccd.support1 = supp1;
2655  ccd.support2 = supp2;
2656 
2657  ccd.max_iterations = max_iterations;
2658  ccd.dist_tolerance = tolerance;
2659  ccd.epa_tolerance = tolerance;
2660 
2661  ccd_vec3_t p1_, p2_;
2662  // NOTE(JS): p1_ and p2_ are set to zeros in order to suppress uninitialized
2663  // warning. It seems the warnings occur since libccd_extension::ccdGJKDist2
2664  // conditionally set p1_ and p2_. If this wasn't intentional then please
2665  // remove the initialization of p1_ and p2_, and change the function
2666  // libccd_extension::ccdGJKDist2(...) to always set p1_ and p2_.
2667  ccdVec3Set(&p1_, 0.0, 0.0, 0.0);
2668  ccdVec3Set(&p2_, 0.0, 0.0, 0.0);
2669  dist = distance_func(obj1, obj2, &ccd, &p1_, &p2_);
2670  if (p1) *p1 << ccdVec3X(&p1_), ccdVec3Y(&p1_), ccdVec3Z(&p1_);
2671  if (p2) *p2 << ccdVec3X(&p2_), ccdVec3Y(&p2_), ccdVec3Z(&p2_);
2672  if (res) *res = dist;
2673  if (dist < 0)
2674  return false;
2675  else
2676  return true;
2677 }
2678 
2679 template <typename S>
2680 bool GJKDistance(void* obj1, ccd_support_fn supp1,
2681  void* obj2, ccd_support_fn supp2,
2682  unsigned int max_iterations, S tolerance,
2683  S* res, Vector3<S>* p1, Vector3<S>* p2) {
2684  return detail::GJKDistanceImpl(obj1, supp1, obj2, supp2, max_iterations,
2686  p1, p2);
2687 }
2688 
2702 template <typename S>
2703 bool GJKSignedDistance(void* obj1, ccd_support_fn supp1,
2704  void* obj2, ccd_support_fn supp2,
2705  unsigned int max_iterations,
2706  S tolerance, S* res, Vector3<S>* p1, Vector3<S>* p2) {
2707  return detail::GJKDistanceImpl(
2708  obj1, supp1, obj2, supp2, max_iterations, tolerance,
2709  libccd_extension::ccdGJKSignedDist, res, p1, p2);
2710 }
2711 
2712 template <typename S>
2714 {
2715  return &supportCyl;
2716 }
2717 
2718 template <typename S>
2720 {
2721  return &centerShape;
2722 }
2723 
2724 template <typename S>
2725 void* GJKInitializer<S, Cylinder<S>>::createGJKObject(const Cylinder<S>& s,
2726  const Transform3<S>& tf)
2727 {
2728  ccd_cyl_t* o = new ccd_cyl_t;
2729  cylToGJK(s, tf, o);
2730  return o;
2731 }
2732 
2733 template <typename S>
2734 void GJKInitializer<S, Cylinder<S>>::deleteGJKObject(void* o_)
2735 {
2736  ccd_cyl_t* o = static_cast<ccd_cyl_t*>(o_);
2737  delete o;
2738 }
2739 
2740 template <typename S>
2742 {
2743  return &supportSphere;
2744 }
2745 
2746 template <typename S>
2748 {
2749  return &centerShape;
2750 }
2751 
2752 template <typename S>
2753 void* GJKInitializer<S, Sphere<S>>::createGJKObject(const Sphere<S>& s,
2754  const Transform3<S>& tf)
2755 {
2756  ccd_sphere_t* o = new ccd_sphere_t;
2757  sphereToGJK(s, tf, o);
2758  return o;
2759 }
2760 
2761 template <typename S>
2762 void GJKInitializer<S, Sphere<S>>::deleteGJKObject(void* o_)
2763 {
2764  ccd_sphere_t* o = static_cast<ccd_sphere_t*>(o_);
2765  delete o;
2766 }
2767 
2768 template <typename S>
2770 {
2771  return &supportEllipsoid;
2772 }
2773 
2774 template <typename S>
2776 {
2777  return &centerShape;
2778 }
2779 
2780 template <typename S>
2781 void* GJKInitializer<S, Ellipsoid<S>>::createGJKObject(const Ellipsoid<S>& s,
2782  const Transform3<S>& tf)
2783 {
2785  ellipsoidToGJK(s, tf, o);
2786  return o;
2787 }
2788 
2789 template <typename S>
2790 void GJKInitializer<S, Ellipsoid<S>>::deleteGJKObject(void* o_)
2791 {
2792  ccd_ellipsoid_t* o = static_cast<ccd_ellipsoid_t*>(o_);
2793  delete o;
2794 }
2795 
2796 template <typename S>
2798 {
2799  return &supportBox;
2800 }
2801 
2802 template <typename S>
2804 {
2805  return &centerShape;
2806 }
2807 
2808 template <typename S>
2809 void* GJKInitializer<S, Box<S>>::createGJKObject(const Box<S>& s,
2810  const Transform3<S>& tf)
2811 {
2812  ccd_box_t* o = new ccd_box_t;
2813  boxToGJK(s, tf, o);
2814  return o;
2815 }
2816 
2817 template <typename S>
2818 void GJKInitializer<S, Box<S>>::deleteGJKObject(void* o_)
2819 {
2820  ccd_box_t* o = static_cast<ccd_box_t*>(o_);
2821  delete o;
2822 }
2823 
2824 template <typename S>
2826 {
2827  return &supportCap;
2828 }
2829 
2830 template <typename S>
2832 {
2833  return &centerShape;
2834 }
2835 
2836 template <typename S>
2837 void* GJKInitializer<S, Capsule<S>>::createGJKObject(const Capsule<S>& s,
2838  const Transform3<S>& tf)
2839 {
2840  ccd_cap_t* o = new ccd_cap_t;
2841  capToGJK(s, tf, o);
2842  return o;
2843 }
2844 
2845 template <typename S>
2846 void GJKInitializer<S, Capsule<S>>::deleteGJKObject(void* o_)
2847 {
2848  ccd_cap_t* o = static_cast<ccd_cap_t*>(o_);
2849  delete o;
2850 }
2851 
2852 template <typename S>
2854 {
2855  return &supportCone;
2856 }
2857 
2858 template <typename S>
2860 {
2861  return &centerShape;
2862 }
2863 
2864 template <typename S>
2865 void* GJKInitializer<S, Cone<S>>::createGJKObject(const Cone<S>& s,
2866  const Transform3<S>& tf)
2867 {
2868  ccd_cone_t* o = new ccd_cone_t;
2869  coneToGJK(s, tf, o);
2870  return o;
2871 }
2872 
2873 template <typename S>
2874 void GJKInitializer<S, Cone<S>>::deleteGJKObject(void* o_)
2875 {
2876  ccd_cone_t* o = static_cast<ccd_cone_t*>(o_);
2877  delete o;
2878 }
2879 
2880 template <typename S>
2882 {
2883  return &supportConvex<S>;
2884 }
2885 
2886 template <typename S>
2888 {
2889  return &centerConvex<S>;
2890 }
2891 
2892 template <typename S>
2893 void* GJKInitializer<S, Convex<S>>::createGJKObject(const Convex<S>& s,
2894  const Transform3<S>& tf)
2895 {
2896  auto* o = new ccd_convex_t<S>;
2897  convexToGJK(s, tf, o);
2898  return o;
2899 }
2900 
2901 template <typename S>
2902 void GJKInitializer<S, Convex<S>>::deleteGJKObject(void* o_)
2903 {
2904  auto* o = static_cast<ccd_convex_t<S>*>(o_);
2905  delete o;
2906 }
2907 
2909 {
2910  return &supportTriangle;
2911 }
2912 
2914 {
2915  return &centerTriangle;
2916 }
2917 
2918 template <typename S>
2919 void* triCreateGJKObject(const Vector3<S>& P1, const Vector3<S>& P2,
2920  const Vector3<S>& P3)
2921 {
2922  ccd_triangle_t* o = new ccd_triangle_t;
2923  Vector3<S> center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3,
2924  (P1[2] + P2[2] + P3[2]) / 3);
2925 
2926  ccdVec3Set(&o->p[0], P1[0], P1[1], P1[2]);
2927  ccdVec3Set(&o->p[1], P2[0], P2[1], P2[2]);
2928  ccdVec3Set(&o->p[2], P3[0], P3[1], P3[2]);
2929  ccdVec3Set(&o->c, center[0], center[1], center[2]);
2930  ccdVec3Set(&o->pos, 0., 0., 0.);
2931  ccdQuatSet(&o->rot, 0., 0., 0., 1.);
2932  ccdQuatInvert2(&o->rot_inv, &o->rot);
2933 
2934  return o;
2935 }
2936 
2937 template <typename S>
2938 void* triCreateGJKObject(const Vector3<S>& P1, const Vector3<S>& P2,
2939  const Vector3<S>& P3, const Transform3<S>& tf)
2940 {
2941  ccd_triangle_t* o = new ccd_triangle_t;
2942  Vector3<S> center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3,
2943  (P1[2] + P2[2] + P3[2]) / 3);
2944 
2945  ccdVec3Set(&o->p[0], P1[0], P1[1], P1[2]);
2946  ccdVec3Set(&o->p[1], P2[0], P2[1], P2[2]);
2947  ccdVec3Set(&o->p[2], P3[0], P3[1], P3[2]);
2948  ccdVec3Set(&o->c, center[0], center[1], center[2]);
2949  const Quaternion<S> q(tf.linear());
2950  const Vector3<S>& T = tf.translation();
2951  ccdVec3Set(&o->pos, T[0], T[1], T[2]);
2952  ccdQuatSet(&o->rot, q.x(), q.y(), q.z(), q.w());
2953  ccdQuatInvert2(&o->rot_inv, &o->rot);
2954 
2955  return o;
2956 }
2957 
2958 inline void triDeleteGJKObject(void* o_)
2959 {
2960  ccd_triangle_t* o = static_cast<ccd_triangle_t*>(o_);
2961  delete o;
2962 }
2963 
2964 } // namespace detail
2965 } // namespace fcl
2966 
2967 #endif
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::detail::libccd_extension::extractObjectPointsFromPoint
static void extractObjectPointsFromPoint(ccd_support_t *q, ccd_vec3_t *p1, ccd_vec3_t *p2)
Definition: gjk_libccd-inl.h:1805
_ccd_pt_t::nearest
ccd_pt_el_t * nearest
Definition: polytope.h:97
_ccd_pt_face_t::edge
__CCD_PT_EL ccd_pt_edge_t * edge[3]
Reference to surrounding edges.
Definition: polytope.h:84
kEps
static const double kEps
Definition: test_gjk_libccd-inl_gjk_doSimplex2.cpp:128
_ccd_pt_vertex_t
Definition: polytope.h:56
fcl::detail::ccd_cap_t::radius
ccd_real_t radius
Definition: gjk_libccd-inl.h:151
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
ccdSimplexSize
_ccd_inline int ccdSimplexSize(const ccd_simplex_t *s)
Definition: simplex.h:54
fcl::detail::libccd_extension::simplexToPolytope4
static int simplexToPolytope4(const void *obj1, const void *obj2, const ccd_t *ccd, ccd_simplex_t *simplex, ccd_pt_t *pt, ccd_pt_el_t **nearest)
Definition: gjk_libccd-inl.h:827
fcl::detail::ccd_cyl_t::radius
ccd_real_t radius
Definition: gjk_libccd-inl.h:156
fcl::Capsule::radius
S radius
Radius of capsule.
Definition: capsule.h:60
ccdPtFaceVec3
_ccd_inline void ccdPtFaceVec3(const ccd_pt_face_t *face, ccd_vec3_t **a, ccd_vec3_t **b, ccd_vec3_t **c)
Definition: polytope.h:251
fcl::detail::libccd_extension::ccdGJKDist2
static ccd_real_t ccdGJKDist2(const void *obj1, const void *obj2, const ccd_t *ccd, ccd_vec3_t *p1, ccd_vec3_t *p2)
Definition: gjk_libccd-inl.h:2249
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
ccdPtAddEdge
ccd_pt_edge_t * ccdPtAddEdge(ccd_pt_t *pt, ccd_pt_vertex_t *v1, ccd_pt_vertex_t *v2)
_ccd_pt_t::faces
ccd_list_t faces
List of faces.
Definition: polytope.h:95
obj2
CollisionObject< S > * obj2
Definition: broadphase_SaP.h:211
fcl::detail::supportSphere
static void supportSphere(const void *obj, const ccd_vec3_t *dir_, ccd_vec3_t *v)
Definition: gjk_libccd-inl.h:2450
fcl::detail::cylToGJK
static void cylToGJK(const Cylinder< S > &s, const Transform3< S > &tf, ccd_cyl_t *cyl)
Definition: gjk_libccd-inl.h:2296
ccdPtNearest
ccd_pt_el_t * ccdPtNearest(ccd_pt_t *pt)
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::ccd_obj_t::rot
ccd_quat_t rot
Definition: gjk_libccd-inl.h:141
fcl::detail::GJKCollide
template bool GJKCollide(void *obj1, ccd_support_fn supp1, ccd_center_fn cen1, void *obj2, ccd_support_fn supp2, ccd_center_fn cen2, unsigned int max_iterations, double tolerance, Vector3d *contact_points, double *penetration_depth, Vector3d *normal)
fcl::detail::shapeToGJK
static void shapeToGJK(const ShapeBase< S > &s, const Transform3< S > &tf, ccd_obj_t *o)
Definition: gjk_libccd-inl.h:2265
fcl::detail::DistanceFn
std::function< ccd_real_t(const void *, const void *, const ccd_t *, ccd_vec3_t *, ccd_vec3_t *)> DistanceFn
Definition: gjk_libccd-inl.h:2622
fcl::detail::libccd_extension::doSimplex3
static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir)
Definition: gjk_libccd-inl.h:388
fcl::Quaternion
Eigen::Quaternion< S > Quaternion
Definition: types.h:88
ccdSimplexSetSize
_ccd_inline void ccdSimplexSetSize(ccd_simplex_t *s, int size)
Definition: simplex.h:86
epsilon
S epsilon()
Definition: test_fcl_simple.cpp:56
_ccd_support_t
Definition: support.h:27
_ccd_simplex_t::ps
ccd_support_t ps[4]
Definition: simplex.h:44
fcl::detail::triDeleteGJKObject
void triDeleteGJKObject(void *o_)
Definition: gjk_libccd-inl.h:2958
fcl::detail::triGetSupportFunction
GJKSupportFunction triGetSupportFunction()
initialize GJK Triangle
Definition: gjk_libccd-inl.h:2908
dir_
ccd_vec3_t dir_
Definition: test_gjk_libccd-inl_gjk_doSimplex2.cpp:114
ccdPtInit
void ccdPtInit(ccd_pt_t *pt)
ccdSimplexSet
_ccd_inline void ccdSimplexSet(ccd_simplex_t *s, size_t pos, const ccd_support_t *a)
Definition: simplex.h:81
fcl::Convex< S >
fcl::constants::eps
static constexpr Real eps()
Returns ε for the precision of the underlying scalar type.
Definition: constants.h:151
fcl::detail::supportEllipsoid
static void supportEllipsoid(const void *obj, const ccd_vec3_t *dir_, ccd_vec3_t *v)
Definition: gjk_libccd-inl.h:2468
fcl::detail::convexToGJK
static void convexToGJK(const Convex< S > &s, const Transform3< S > &tf, ccd_convex_t< S > *conv)
Definition: gjk_libccd-inl.h:2332
fcl::detail::centerShape
static void centerShape(const void *obj, ccd_vec3_t *c)
Definition: gjk_libccd-inl.h:2550
ccdSimplexLast
const _ccd_inline ccd_support_t * ccdSimplexLast(const ccd_simplex_t *s)
Definition: simplex.h:59
fcl::detail::supportBox
static void supportBox(const void *obj, const ccd_vec3_t *dir_, ccd_vec3_t *v)
Definition: gjk_libccd-inl.h:2340
fcl::detail::libccd_extension::tripleCross
_ccd_inline void tripleCross(const ccd_vec3_t *a, const ccd_vec3_t *b, const ccd_vec3_t *c, ccd_vec3_t *d)
Definition: gjk_libccd-inl.h:223
fcl::detail::libccd_extension::__ccdEPA
static int __ccdEPA(const void *obj1, const void *obj2, const ccd_t *ccd, ccd_simplex_t *simplex, ccd_pt_t *polytope, ccd_pt_el_t **nearest)
Definition: gjk_libccd-inl.h:1747
_ccd_support_t::v
ccd_vec3_t v
Support point in minkowski sum.
Definition: support.h:43
fcl::detail::are_coincident
bool are_coincident(const Vector3d &p, const Vector3d &q)
Definition: test_gjk_libccd-inl_extractClosestPoints.cpp:115
fcl::Transform3d
Transform3< double > Transform3d
Definition: types.h:117
unused.h
obj1
CollisionObject< S > * obj1
Definition: broadphase_SaP.h:210
fcl::Ellipsoid::radii
Vector3< S > radii
Radii of the ellipsoid.
Definition: ellipsoid.h:63
fcl::detail::libccd_extension::extractClosestPoints
static void extractClosestPoints(ccd_simplex_t *simplex, ccd_vec3_t *p1, ccd_vec3_t *p2, ccd_vec3_t *p)
Definition: gjk_libccd-inl.h:1894
max
static T max(T x, T y)
Definition: svm.cpp:52
fcl::detail::libccd_extension::_ccdDist
static ccd_real_t _ccdDist(const void *obj1, const void *obj2, const ccd_t *ccd, ccd_simplex_t *simplex, ccd_vec3_t *p1, ccd_vec3_t *p2)
Definition: gjk_libccd-inl.h:2035
fcl::detail::ccd_convex_t
Definition: gjk_libccd-inl.h:175
fcl::detail::libccd_extension::doSimplex2
static int doSimplex2(ccd_simplex_t *simplex, ccd_vec3_t *dir)
Definition: gjk_libccd-inl.h:313
fcl::Capsule::lz
S lz
Length along z axis.
Definition: capsule.h:63
fcl::detail::capToGJK
static void capToGJK(const Capsule< S > &s, const Transform3< S > &tf, ccd_cap_t *cap)
Definition: gjk_libccd-inl.h:2287
fcl::ShapeBase< S >
fcl::detail::GJKInitializer
initialize GJK stuffs
Definition: gjk_libccd.h:76
obj
CollisionObject< S > * obj
object
Definition: broadphase_SaP.h:164
fcl::detail::ccd_obj_t
Definition: gjk_libccd-inl.h:138
_ccd_support_t::v1
ccd_vec3_t v1
Support point in obj1.
Definition: support.h:44
fcl::detail::ccd_triangle_t::c
ccd_vec3_t c
Definition: gjk_libccd-inl.h:183
CCD_PT_EDGE
#define CCD_PT_EDGE
Definition: polytope.h:31
failed_at_this_configuration.h
ccdPtEdgeVec3
_ccd_inline void ccdPtEdgeVec3(const ccd_pt_edge_t *e, ccd_vec3_t **a, ccd_vec3_t **b)
Definition: polytope.h:293
fcl::detail::libccd_extension::doSimplex
static int doSimplex(ccd_simplex_t *simplex, ccd_vec3_t *dir)
Definition: gjk_libccd-inl.h:573
CCD_PT_VERTEX
#define CCD_PT_VERTEX
Definition: polytope.h:30
warning.h
ccdListForEachEntry
#define ccdListForEachEntry(head, pos, postype, member)
Definition: list.h:66
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
ccdSimplexPoint
const _ccd_inline ccd_support_t * ccdSimplexPoint(const ccd_simplex_t *s, int idx)
Definition: simplex.h:64
_ccd_support_t::v2
ccd_vec3_t v2
Support point in obj2.
Definition: support.h:45
fcl::detail::ccd_cone_t::radius
ccd_real_t radius
Definition: gjk_libccd-inl.h:161
fcl::detail::libccd_extension::ClassifyInternalEdge
static void ClassifyInternalEdge(ccd_pt_edge_t *edge, std::unordered_set< ccd_pt_edge_t * > *border_edges, std::unordered_set< ccd_pt_edge_t * > *internal_edges)
Definition: gjk_libccd-inl.h:1188
fcl::detail::ccd_box_t
Definition: gjk_libccd-inl.h:144
fcl::detail::sphereToGJK
static void sphereToGJK(const Sphere< S > &s, const Transform3< S > &tf, ccd_sphere_t *sph)
Definition: gjk_libccd-inl.h:2314
fcl::detail::libccd_extension::extractObjectPointsFromSegment
static void extractObjectPointsFromSegment(ccd_support_t *a, ccd_support_t *b, ccd_vec3_t *p1, ccd_vec3_t *p2, ccd_vec3_t *p)
Definition: gjk_libccd-inl.h:1818
fcl::detail::libccd_extension::isAbsValueLessThanEpsSquared
static bool isAbsValueLessThanEpsSquared(ccd_real_t val)
Definition: gjk_libccd-inl.h:379
fcl::detail::ccd_box_t::dim
ccd_real_t dim[3]
Definition: gjk_libccd-inl.h:146
fcl::detail::centerTriangle
static void centerTriangle(const void *obj, ccd_vec3_t *c)
Definition: gjk_libccd-inl.h:2566
_ccd_simplex_t
Definition: simplex.h:28
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
fcl::Cone::lz
S lz
Length along z axis.
Definition: cone.h:62
fcl::detail::ccd_obj_t::rot_inv
ccd_quat_t rot_inv
Definition: gjk_libccd-inl.h:141
fcl::detail::triCreateGJKObject
template void * triCreateGJKObject(const Vector3d &P1, const Vector3d &P2, const Vector3d &P3)
fcl::detail::ccd_triangle_t::p
ccd_vec3_t p[3]
Definition: gjk_libccd-inl.h:182
fcl::detail::ccd_cap_t::height
ccd_real_t height
Definition: gjk_libccd-inl.h:151
fcl::detail::libccd_extension::simplexToPolytope2
static int simplexToPolytope2(const void *obj1, const void *obj2, const ccd_t *ccd, const ccd_simplex_t *simplex, ccd_pt_t *pt, ccd_pt_el_t **nearest)
Definition: gjk_libccd-inl.h:589
fcl::detail::ccd_ellipsoid_t
Definition: gjk_libccd-inl.h:169
_ccd_pt_edge_t
Definition: polytope.h:68
fcl::detail::ccd_sphere_t
Definition: gjk_libccd-inl.h:164
_ccd_pt_el_t
Definition: polytope.h:45
fcl::detail::centerConvex
static void centerConvex(const void *obj, ccd_vec3_t *c)
Definition: gjk_libccd-inl.h:2557
FCL_THROW_FAILED_AT_THIS_CONFIGURATION
#define FCL_THROW_FAILED_AT_THIS_CONFIGURATION(message)
Definition: failed_at_this_configuration.h:121
_ccd_pt_face_t
Definition: polytope.h:81
fcl::detail::libccd_extension::ccdGJKSignedDist
static ccd_real_t ccdGJKSignedDist(const void *obj1, const void *obj2, const ccd_t *ccd, ccd_vec3_t *p1, ccd_vec3_t *p2)
Definition: gjk_libccd-inl.h:2186
_ccd_pt_t::nearest_type
int nearest_type
Definition: polytope.h:99
ccdSimplexInit
_ccd_inline void ccdSimplexInit(ccd_simplex_t *s)
Definition: simplex.h:49
fcl::detail::coneToGJK
static void coneToGJK(const Cone< S > &s, const Transform3< S > &tf, ccd_cone_t *cone)
Definition: gjk_libccd-inl.h:2305
fcl::Sphere::radius
S radius
Radius of the sphere.
Definition: sphere.h:59
fcl::detail::ccd_triangle_t
Definition: gjk_libccd-inl.h:180
ccdPtEdgeFaces
_ccd_inline void ccdPtEdgeFaces(const ccd_pt_edge_t *e, ccd_pt_face_t **f1, ccd_pt_face_t **f2)
Definition: polytope.h:309
fcl::detail::GJKSupportFunction
void(*)(const void *obj, const ccd_vec3_t *dir_, ccd_vec3_t *v) GJKSupportFunction
callback function used by GJK algorithm
Definition: gjk_libccd.h:71
fcl::Cone::radius
S radius
Radius of the cone.
Definition: cone.h:59
fcl::detail::libccd_extension::__ccdGJK
static int __ccdGJK(const void *obj1, const void *obj2, const ccd_t *ccd, ccd_simplex_t *simplex)
Definition: gjk_libccd-inl.h:1597
fcl::detail::ccd_sphere_t::radius
ccd_real_t radius
Definition: gjk_libccd-inl.h:166
gjk_libccd.h
ccdPtDestroy
void ccdPtDestroy(ccd_pt_t *pt)
Vector3d
fcl::Vector3d Vector3d
Definition: test_broadphase_dynamic_AABB_tree.cpp:48
ccdPtDelEdge
_ccd_inline int ccdPtDelEdge(ccd_pt_t *pt, ccd_pt_edge_t *)
Definition: polytope.h:204
fcl::Cylinder
Center at zero cylinder.
Definition: cylinder.h:50
fcl::detail::supportConvex
static void supportConvex(const void *obj, const ccd_vec3_t *dir_, ccd_vec3_t *v)
Definition: gjk_libccd-inl.h:2494
fcl::detail::ccd_ellipsoid_t::radii
ccd_real_t radii[3]
Definition: gjk_libccd-inl.h:171
fcl::detail::libccd_extension::isPolytopeEmpty
static bool isPolytopeEmpty(const ccd_pt_t &polytope)
Definition: gjk_libccd-inl.h:695
fcl::detail::ccd_cone_t::height
ccd_real_t height
Definition: gjk_libccd-inl.h:161
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::libccd_extension::doSimplex4
static int doSimplex4(ccd_simplex_t *simplex, ccd_vec3_t *dir)
Definition: gjk_libccd-inl.h:479
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
fcl::detail::libccd_extension::ClassifyBorderEdge
static void ClassifyBorderEdge(ccd_pt_edge_t *edge, std::unordered_set< ccd_pt_edge_t * > *border_edges, std::unordered_set< ccd_pt_edge_t * > *internal_edges)
Definition: gjk_libccd-inl.h:1170
ccdPtDelFace
_ccd_inline int ccdPtDelFace(ccd_pt_t *pt, ccd_pt_face_t *)
Definition: polytope.h:226
fcl::detail::ccd_cyl_t::height
ccd_real_t height
Definition: gjk_libccd-inl.h:156
fcl::detail::ccd_cone_t
Definition: gjk_libccd-inl.h:159
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:50
_ccd_simplex_t::last
int last
index of last added point
Definition: simplex.h:45
fcl::detail::supportCap
static void supportCap(const void *obj, const ccd_vec3_t *dir_, ccd_vec3_t *v)
Definition: gjk_libccd-inl.h:2362
FCL_UNUSED
#define FCL_UNUSED(x)
Definition: unused.h:39
_ccd_pt_t::nearest_dist
ccd_real_t nearest_dist
Definition: polytope.h:98
fcl::detail::libccd_extension::simplexReduceToTriangle
static ccd_real_t simplexReduceToTriangle(ccd_simplex_t *simplex, ccd_real_t dist, ccd_vec3_t *best_witness)
Definition: gjk_libccd-inl.h:189
fcl::detail::libccd_extension::validateNearestFeatureOfPolytopeBeingEdge
static void validateNearestFeatureOfPolytopeBeingEdge(ccd_pt_t *polytope)
Definition: gjk_libccd-inl.h:1670
fcl::Cylinder::lz
S lz
Length along z axis.
Definition: cylinder.h:63
fcl::Sphere
Center at zero point sphere.
Definition: sphere.h:50
fcl::detail::ccd_convex_t::convex
const Convex< S > * convex
Definition: gjk_libccd-inl.h:177
ccdSupportCopy
_ccd_inline void ccdSupportCopy(ccd_support_t *, const ccd_support_t *s)
Definition: support.h:46
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::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::Convex::getInteriorPoint
const Vector3< S > & getInteriorPoint() const
A point guaranteed to be on the interior of the convex polytope, used for collision.
Definition: convex.h:155
min
static T min(T x, T y)
Definition: svm.cpp:49
tolerance
S tolerance()
Definition: test_fcl_geometric_shapes.cpp:87
ccdPtAddVertex
ccd_pt_vertex_t * ccdPtAddVertex(ccd_pt_t *pt, const ccd_support_t *v)
fcl::detail::supportCone
static void supportCone(const void *obj, const ccd_vec3_t *dir_, ccd_vec3_t *v)
Definition: gjk_libccd-inl.h:2418
_ccd_pt_vertex_t::v
ccd_support_t v
Definition: polytope.h:60
fcl::constants
Definition: constants.h:129
_ccd_pt_t
Definition: polytope.h:92
fcl::detail::GJKDistance
template bool GJKDistance(void *obj1, ccd_support_fn supp1, void *obj2, ccd_support_fn supp2, unsigned int max_iterations, double tolerance, double *dist, Vector3d *p1, Vector3d *p2)
CCD_PT_FACE
#define CCD_PT_FACE
Definition: polytope.h:32
fcl::detail::triGetCenterFunction
GJKCenterFunction triGetCenterFunction()
Definition: gjk_libccd-inl.h:2913
fcl::detail::libccd_extension::nextSupport
static int nextSupport(const ccd_pt_t *polytope, const void *obj1, const void *obj2, const ccd_t *ccd, const ccd_pt_el_t *el, ccd_support_t *out)
Definition: gjk_libccd-inl.h:1553
fcl::detail::GJKDistanceImpl
bool GJKDistanceImpl(void *obj1, ccd_support_fn supp1, void *obj2, ccd_support_fn supp2, unsigned int max_iterations, S tolerance, detail::DistanceFn distance_func, S *res, Vector3< S > *p1, Vector3< S > *p2)
Definition: gjk_libccd-inl.h:2647
fcl::Cone
Center at zero cone.
Definition: cone.h:50
fcl::detail::ccd_cyl_t
Definition: gjk_libccd-inl.h:154
fcl::detail::GJKSignedDistance
template bool GJKSignedDistance(void *obj1, ccd_support_fn supp1, void *obj2, ccd_support_fn supp2, unsigned int max_iterations, double tolerance, double *dist, Vector3d *p1, Vector3d *p2)
fcl::detail::supportTriangle
static void supportTriangle(const void *obj, const ccd_vec3_t *dir_, ccd_vec3_t *v)
Definition: gjk_libccd-inl.h:2520
fcl::detail::supportCyl
static void supportCyl(const void *obj, const ccd_vec3_t *dir_, ccd_vec3_t *v)
Definition: gjk_libccd-inl.h:2390
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
__ccdSupport
void __ccdSupport(const void *obj1, const void *obj2, const ccd_vec3_t *dir, const ccd_t *ccd, ccd_support_t *supp)
_ccd_pt_t::edges
ccd_list_t edges
List of edges.
Definition: polytope.h:94
fcl::detail::boxToGJK
static void boxToGJK(const Box< S > &s, const Transform3< S > &tf, ccd_box_t *box)
Definition: gjk_libccd-inl.h:2278
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::ccd_obj_t::pos
ccd_vec3_t pos
Definition: gjk_libccd-inl.h:140
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::ellipsoidToGJK
static void ellipsoidToGJK(const Ellipsoid< S > &s, const Transform3< S > &tf, ccd_ellipsoid_t *ellipsoid)
Definition: gjk_libccd-inl.h:2322
fcl::Box::side
Vector3< S > side
box side length
Definition: box.h:66
fcl::Capsule< S >
fcl::Ellipsoid
Center at zero point ellipsoid.
Definition: ellipsoid.h:50
_ccd_pt_t::vertices
ccd_list_t vertices
List of vertices.
Definition: polytope.h:93
fcl::detail::GJKCenterFunction
void(*)(const void *obj, ccd_vec3_t *c) GJKCenterFunction
Definition: gjk_libccd.h:72
fcl::detail::ccd_cap_t
Definition: gjk_libccd-inl.h:149
fcl::Cylinder::radius
S radius
Radius of the cylinder.
Definition: cylinder.h:60
fcl::detail::triangle_area_is_zero
bool triangle_area_is_zero(const Vector3d &a, const Vector3d &b, const Vector3d &c)
Definition: test_gjk_libccd-inl_extractClosestPoints.cpp:177
_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:00