FCL  0.6.0
Flexible Collision Library
gjk_libccd-inl.h
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 
41 #include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd.h"
42 
43 namespace fcl
44 {
45 
46 namespace detail
47 {
48 
49 //==============================================================================
50 extern template
51 class GJKInitializer<double, Cylinder<double>>;
52 
53 //==============================================================================
54 extern template
55 class GJKInitializer<double, Sphere<double>>;
56 
57 //==============================================================================
58 extern template
59 class GJKInitializer<double, Ellipsoid<double>>;
60 
61 //==============================================================================
62 extern template
63 class GJKInitializer<double, Box<double>>;
64 
65 //==============================================================================
66 extern template
67 class GJKInitializer<double, Capsule<double>>;
68 
69 //==============================================================================
70 extern template
71 class GJKInitializer<double, Cone<double>>;
72 
73 //==============================================================================
74 extern template
75 class GJKInitializer<double, Convex<double>>;
76 
77 //==============================================================================
78 extern template
79 void* triCreateGJKObject(
80  const Vector3d& P1, const Vector3d& P2, const Vector3d& P3);
81 
82 //==============================================================================
83 extern template
84 void* triCreateGJKObject(
85  const Vector3d& P1,
86  const Vector3d& P2,
87  const Vector3d& P3,
88  const Transform3d& tf);
89 
90 //==============================================================================
91 extern template
92 bool GJKCollide(
93  void* obj1,
94  ccd_support_fn supp1,
95  ccd_center_fn cen1,
96  void* obj2,
97  ccd_support_fn supp2,
98  ccd_center_fn cen2,
99  unsigned int max_iterations,
100  double tolerance,
101  Vector3d* contact_points,
102  double* penetration_depth,
103  Vector3d* normal);
104 
105 //==============================================================================
106 extern template
107 bool GJKDistance(
108  void* obj1,
109  ccd_support_fn supp1,
110  void* obj2,
111  ccd_support_fn supp2,
112  unsigned int max_iterations,
113  double tolerance,
114  double* dist,
115  Vector3d* p1,
116  Vector3d* p2);
117 
118 struct ccd_obj_t
119 {
120  ccd_vec3_t pos;
121  ccd_quat_t rot, rot_inv;
122 };
123 
124 struct ccd_box_t : public ccd_obj_t
125 {
126  ccd_real_t dim[3];
127 };
128 
129 struct ccd_cap_t : public ccd_obj_t
130 {
131  ccd_real_t radius, height;
132 };
133 
134 struct ccd_cyl_t : public ccd_obj_t
135 {
136  ccd_real_t radius, height;
137 };
138 
139 struct ccd_cone_t : public ccd_obj_t
140 {
141  ccd_real_t radius, height;
142 };
143 
144 struct ccd_sphere_t : public ccd_obj_t
145 {
146  ccd_real_t radius;
147 };
148 
149 struct ccd_ellipsoid_t : public ccd_obj_t
150 {
151  ccd_real_t radii[3];
152 };
153 
154 template <typename S>
155 struct ccd_convex_t : public ccd_obj_t
156 {
157  const Convex<S>* convex;
158 };
159 
160 struct ccd_triangle_t : public ccd_obj_t
161 {
162  ccd_vec3_t p[3];
163  ccd_vec3_t c;
164 };
165 
166 namespace libccd_extension
167 {
168 
169 static ccd_real_t simplexReduceToTriangle(ccd_simplex_t *simplex,
170  ccd_real_t dist,
171  ccd_vec3_t *best_witness)
172 {
173  ccd_real_t newdist;
174  ccd_vec3_t witness;
175  int best = -1;
176  int i;
177 
178  // try the fourth point in all three positions
179  for (i = 0; i < 3; i++){
180  newdist = ccdVec3PointTriDist2(ccd_vec3_origin,
181  &ccdSimplexPoint(simplex, (i == 0 ? 3 : 0))->v,
182  &ccdSimplexPoint(simplex, (i == 1 ? 3 : 1))->v,
183  &ccdSimplexPoint(simplex, (i == 2 ? 3 : 2))->v,
184  &witness);
185  newdist = CCD_SQRT(newdist);
186 
187  // record the best triangle
188  if (newdist < dist){
189  dist = newdist;
190  best = i;
191  ccdVec3Copy(best_witness, &witness);
192  }
193  }
194 
195  if (best >= 0){
196  ccdSimplexSet(simplex, best, ccdSimplexPoint(simplex, 3));
197  }
198  ccdSimplexSetSize(simplex, 3);
199 
200  return dist;
201 }
202 
203 _ccd_inline void tripleCross(const ccd_vec3_t *a, const ccd_vec3_t *b,
204  const ccd_vec3_t *c, ccd_vec3_t *d)
205 {
206  ccd_vec3_t e;
207  ccdVec3Cross(&e, a, b);
208  ccdVec3Cross(d, &e, c);
209 }
210 
211 static int doSimplex2(ccd_simplex_t *simplex, ccd_vec3_t *dir)
212 {
213  const ccd_support_t *A, *B;
214  ccd_vec3_t AB, AO, tmp;
215  ccd_real_t dot;
216 
217  // get last added as A
218  A = ccdSimplexLast(simplex);
219  // get the other point
220  B = ccdSimplexPoint(simplex, 0);
221  // compute AB oriented segment
222  ccdVec3Sub2(&AB, &B->v, &A->v);
223  // compute AO vector
224  ccdVec3Copy(&AO, &A->v);
225  ccdVec3Scale(&AO, -CCD_ONE);
226 
227  // dot product AB . AO
228  dot = ccdVec3Dot(&AB, &AO);
229 
230  // check if origin doesn't lie on AB segment
231  ccdVec3Cross(&tmp, &AB, &AO);
232  if (ccdIsZero(ccdVec3Len2(&tmp)) && dot > CCD_ZERO){
233  return 1;
234  }
235 
236  // check if origin is in area where AB segment is
237  if (ccdIsZero(dot) || dot < CCD_ZERO){
238  // origin is in outside are of A
239  ccdSimplexSet(simplex, 0, A);
240  ccdSimplexSetSize(simplex, 1);
241  ccdVec3Copy(dir, &AO);
242  }else{
243  // origin is in area where AB segment is
244 
245  // keep simplex untouched and set direction to
246  // AB x AO x AB
247  tripleCross(&AB, &AO, &AB, dir);
248  }
249 
250  return 0;
251 }
252 
253 static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir)
254 {
255  const ccd_support_t *A, *B, *C;
256  ccd_vec3_t AO, AB, AC, ABC, tmp;
257  ccd_real_t dot, dist;
258 
259  // get last added as A
260  A = ccdSimplexLast(simplex);
261  // get the other points
262  B = ccdSimplexPoint(simplex, 1);
263  C = ccdSimplexPoint(simplex, 0);
264 
265  // check touching contact
266  dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, nullptr);
267  if (ccdIsZero(dist)){
268  return 1;
269  }
270 
271  // check if triangle is really triangle (has area > 0)
272  // if not simplex can't be expanded and thus no itersection is found
273  if (ccdVec3Eq(&A->v, &B->v) || ccdVec3Eq(&A->v, &C->v)){
274  return -1;
275  }
276 
277  // compute AO vector
278  ccdVec3Copy(&AO, &A->v);
279  ccdVec3Scale(&AO, -CCD_ONE);
280 
281  // compute AB and AC segments and ABC vector (perpendircular to triangle)
282  ccdVec3Sub2(&AB, &B->v, &A->v);
283  ccdVec3Sub2(&AC, &C->v, &A->v);
284  ccdVec3Cross(&ABC, &AB, &AC);
285 
286  ccdVec3Cross(&tmp, &ABC, &AC);
287  dot = ccdVec3Dot(&tmp, &AO);
288  if (ccdIsZero(dot) || dot > CCD_ZERO){
289  dot = ccdVec3Dot(&AC, &AO);
290  if (ccdIsZero(dot) || dot > CCD_ZERO){
291  // C is already in place
292  ccdSimplexSet(simplex, 1, A);
293  ccdSimplexSetSize(simplex, 2);
294  tripleCross(&AC, &AO, &AC, dir);
295  }else{
296  ccd_do_simplex3_45:
297  dot = ccdVec3Dot(&AB, &AO);
298  if (ccdIsZero(dot) || dot > CCD_ZERO){
299  ccdSimplexSet(simplex, 0, B);
300  ccdSimplexSet(simplex, 1, A);
301  ccdSimplexSetSize(simplex, 2);
302  tripleCross(&AB, &AO, &AB, dir);
303  }else{
304  ccdSimplexSet(simplex, 0, A);
305  ccdSimplexSetSize(simplex, 1);
306  ccdVec3Copy(dir, &AO);
307  }
308  }
309  }else{
310  ccdVec3Cross(&tmp, &AB, &ABC);
311  dot = ccdVec3Dot(&tmp, &AO);
312  if (ccdIsZero(dot) || dot > CCD_ZERO){
313  goto ccd_do_simplex3_45;
314  }else{
315  dot = ccdVec3Dot(&ABC, &AO);
316  if (ccdIsZero(dot) || dot > CCD_ZERO){
317  ccdVec3Copy(dir, &ABC);
318  }else{
319  ccd_support_t Ctmp;
320  ccdSupportCopy(&Ctmp, C);
321  ccdSimplexSet(simplex, 0, B);
322  ccdSimplexSet(simplex, 1, &Ctmp);
323 
324  ccdVec3Copy(dir, &ABC);
325  ccdVec3Scale(dir, -CCD_ONE);
326  }
327  }
328  }
329 
330  return 0;
331 }
332 
333 static int doSimplex4(ccd_simplex_t *simplex, ccd_vec3_t *dir)
334 {
335  const ccd_support_t *A, *B, *C, *D;
336  ccd_vec3_t AO, AB, AC, AD, ABC, ACD, ADB;
337  int B_on_ACD, C_on_ADB, D_on_ABC;
338  int AB_O, AC_O, AD_O;
339  ccd_real_t dist;
340 
341  // get last added as A
342  A = ccdSimplexLast(simplex);
343  // get the other points
344  B = ccdSimplexPoint(simplex, 2);
345  C = ccdSimplexPoint(simplex, 1);
346  D = ccdSimplexPoint(simplex, 0);
347 
348  // check if tetrahedron is really tetrahedron (has volume > 0)
349  // if it is not simplex can't be expanded and thus no intersection is
350  // found
351  dist = ccdVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, nullptr);
352  if (ccdIsZero(dist)){
353  return -1;
354  }
355 
356  // check if origin lies on some of tetrahedron's face - if so objects
357  // intersect
358  dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, nullptr);
359  if (ccdIsZero(dist))
360  return 1;
361  dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &C->v, &D->v, nullptr);
362  if (ccdIsZero(dist))
363  return 1;
364  dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &D->v, nullptr);
365  if (ccdIsZero(dist))
366  return 1;
367  dist = ccdVec3PointTriDist2(ccd_vec3_origin, &B->v, &C->v, &D->v, nullptr);
368  if (ccdIsZero(dist))
369  return 1;
370 
371  // compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors
372  ccdVec3Copy(&AO, &A->v);
373  ccdVec3Scale(&AO, -CCD_ONE);
374  ccdVec3Sub2(&AB, &B->v, &A->v);
375  ccdVec3Sub2(&AC, &C->v, &A->v);
376  ccdVec3Sub2(&AD, &D->v, &A->v);
377  ccdVec3Cross(&ABC, &AB, &AC);
378  ccdVec3Cross(&ACD, &AC, &AD);
379  ccdVec3Cross(&ADB, &AD, &AB);
380 
381  // side (positive or negative) of B, C, D relative to planes ACD, ADB
382  // and ABC respectively
383  B_on_ACD = ccdSign(ccdVec3Dot(&ACD, &AB));
384  C_on_ADB = ccdSign(ccdVec3Dot(&ADB, &AC));
385  D_on_ABC = ccdSign(ccdVec3Dot(&ABC, &AD));
386 
387  // whether origin is on same side of ACD, ADB, ABC as B, C, D
388  // respectively
389  AB_O = ccdSign(ccdVec3Dot(&ACD, &AO)) == B_on_ACD;
390  AC_O = ccdSign(ccdVec3Dot(&ADB, &AO)) == C_on_ADB;
391  AD_O = ccdSign(ccdVec3Dot(&ABC, &AO)) == D_on_ABC;
392 
393  if (AB_O && AC_O && AD_O){
394  // origin is in tetrahedron
395  return 1;
396 
397  // rearrange simplex to triangle and call doSimplex3()
398  }else if (!AB_O){
399  // B is farthest from the origin among all of the tetrahedron's
400  // points, so remove it from the list and go on with the triangle
401  // case
402 
403  // D and C are in place
404  ccdSimplexSet(simplex, 2, A);
405  ccdSimplexSetSize(simplex, 3);
406  }else if (!AC_O){
407  // C is farthest
408  ccdSimplexSet(simplex, 1, D);
409  ccdSimplexSet(simplex, 0, B);
410  ccdSimplexSet(simplex, 2, A);
411  ccdSimplexSetSize(simplex, 3);
412  }else{ // (!AD_O)
413  ccdSimplexSet(simplex, 0, C);
414  ccdSimplexSet(simplex, 1, B);
415  ccdSimplexSet(simplex, 2, A);
416  ccdSimplexSetSize(simplex, 3);
417  }
418 
419  return doSimplex3(simplex, dir);
420 }
421 
422 static int doSimplex(ccd_simplex_t *simplex, ccd_vec3_t *dir)
423 {
424  if (ccdSimplexSize(simplex) == 2){
425  // simplex contains segment only one segment
426  return doSimplex2(simplex, dir);
427  }else if (ccdSimplexSize(simplex) == 3){
428  // simplex contains triangle
429  return doSimplex3(simplex, dir);
430  }else{ // ccdSimplexSize(simplex) == 4
431  // tetrahedron - this is the only shape which can encapsule origin
432  // so doSimplex4() also contains test on it
433  return doSimplex4(simplex, dir);
434  }
435 }
436 
437 static int __ccdGJK(const void *obj1, const void *obj2,
438  const ccd_t *ccd, ccd_simplex_t *simplex)
439 {
440  unsigned long iterations;
441  ccd_vec3_t dir; // direction vector
442  ccd_support_t last; // last support point
443  int do_simplex_res;
444 
445  // initialize simplex struct
446  ccdSimplexInit(simplex);
447 
448  // get first direction
449  ccd->first_dir(obj1, obj2, &dir);
450  // get first support point
451  __ccdSupport(obj1, obj2, &dir, ccd, &last);
452  // and add this point to simplex as last one
453  ccdSimplexAdd(simplex, &last);
454 
455  // set up direction vector to as (O - last) which is exactly -last
456  ccdVec3Copy(&dir, &last.v);
457  ccdVec3Scale(&dir, -CCD_ONE);
458 
459  // start iterations
460  for (iterations = 0UL; iterations < ccd->max_iterations; ++iterations) {
461  // obtain support point
462  __ccdSupport(obj1, obj2, &dir, ccd, &last);
463 
464  // check if farthest point in Minkowski difference in direction dir
465  // isn't somewhere before origin (the test on negative dot product)
466  // - because if it is, objects are not intersecting at all.
467  if (ccdVec3Dot(&last.v, &dir) < CCD_ZERO){
468  return -1; // intersection not found
469  }
470 
471  // add last support vector to simplex
472  ccdSimplexAdd(simplex, &last);
473 
474  // if doSimplex returns 1 if objects intersect, -1 if objects don't
475  // intersect and 0 if algorithm should continue
476  do_simplex_res = doSimplex(simplex, &dir);
477  if (do_simplex_res == 1){
478  return 0; // intersection found
479  }else if (do_simplex_res == -1){
480  return -1; // intersection not found
481  }
482 
483  if (ccdIsZero(ccdVec3Len2(&dir))){
484  return -1; // intersection not found
485  }
486  }
487 
488  // intersection wasn't found
489  return -1;
490 }
491 
493 static inline ccd_real_t ccdGJKDist2(const void *obj1, const void *obj2, const ccd_t *ccd, ccd_vec3_t* p1, ccd_vec3_t* p2)
494 {
495  unsigned long iterations;
496  ccd_simplex_t simplex;
497  ccd_support_t last; // last support point
498  ccd_vec3_t dir; // direction vector
499  ccd_real_t dist, last_dist;
500 
501  // first find an intersection
502  if (__ccdGJK(obj1, obj2, ccd, &simplex) == 0)
503  return -CCD_ONE;
504 
505  last_dist = CCD_REAL_MAX;
506 
507  for (iterations = 0UL; iterations < ccd->max_iterations; ++iterations) {
508  // get a next direction vector
509  // we are trying to find out a point on the minkowski difference
510  // that is nearest to the origin, so we obtain a point on the
511  // simplex that is nearest and try to exapand the simplex towards
512  // the origin
513  if (ccdSimplexSize(&simplex) == 1){
514  ccdVec3Copy(&dir, &ccdSimplexPoint(&simplex, 0)->v);
515  dist = ccdVec3Len2(&ccdSimplexPoint(&simplex, 0)->v);
516  dist = CCD_SQRT(dist);
517  }else if (ccdSimplexSize(&simplex) == 2){
518  dist = ccdVec3PointSegmentDist2(ccd_vec3_origin,
519  &ccdSimplexPoint(&simplex, 0)->v,
520  &ccdSimplexPoint(&simplex, 1)->v,
521  &dir);
522  dist = CCD_SQRT(dist);
523  }else if(ccdSimplexSize(&simplex) == 3){
524  dist = ccdVec3PointTriDist2(ccd_vec3_origin,
525  &ccdSimplexPoint(&simplex, 0)->v,
526  &ccdSimplexPoint(&simplex, 1)->v,
527  &ccdSimplexPoint(&simplex, 2)->v,
528  &dir);
529  dist = CCD_SQRT(dist);
530  }else{ // ccdSimplexSize(&simplex) == 4
531  dist = simplexReduceToTriangle(&simplex, last_dist, &dir);
532  }
533 
534  // touching contact -- do we really need this?
535  // maybe __ccdGJK() solve this alredy.
536  if (ccdIsZero(dist))
537  return -CCD_ONE;
538 
539  // check whether we improved for at least a minimum tolerance
540  if ((last_dist - dist) < ccd->dist_tolerance)
541  {
542  if(p1) *p1 = last.v1;
543  if(p2) *p2 = last.v2;
544  return dist;
545  }
546 
547  // point direction towards the origin
548  ccdVec3Scale(&dir, -CCD_ONE);
549  ccdVec3Normalize(&dir);
550 
551  // find out support point
552  __ccdSupport(obj1, obj2, &dir, ccd, &last);
553 
554  // record last distance
555  last_dist = dist;
556 
557  // check whether we improved for at least a minimum tolerance
558  // this is here probably only for a degenerate cases when we got a
559  // point that is already in the simplex
560  dist = ccdVec3Len2(&last.v);
561  dist = CCD_SQRT(dist);
562  if (CCD_FABS(last_dist - dist) < ccd->dist_tolerance)
563  {
564  if(p1) *p1 = last.v1;
565  if(p2) *p2 = last.v2;
566  return last_dist;
567  }
568 
569  // add a point to simplex
570  ccdSimplexAdd(&simplex, &last);
571  }
572 
573  return -CCD_REAL(1.);
574 }
575 
576 } // namespace libccd_extension
577 
579 template <typename S>
580 static void shapeToGJK(const ShapeBase<S>& s, const Transform3<S>& tf, ccd_obj_t* o)
581 {
582  const Quaternion<S> q(tf.linear());
583  const Vector3<S>& T = tf.translation();
584  ccdVec3Set(&o->pos, T[0], T[1], T[2]);
585  ccdQuatSet(&o->rot, q.x(), q.y(), q.z(), q.w());
586  ccdQuatInvert2(&o->rot_inv, &o->rot);
587 }
588 
589 template <typename S>
590 static void boxToGJK(const Box<S>& s, const Transform3<S>& tf, ccd_box_t* box)
591 {
592  shapeToGJK(s, tf, box);
593  box->dim[0] = s.side[0] / 2.0;
594  box->dim[1] = s.side[1] / 2.0;
595  box->dim[2] = s.side[2] / 2.0;
596 }
597 
598 template <typename S>
599 static void capToGJK(const Capsule<S>& s, const Transform3<S>& tf, ccd_cap_t* cap)
600 {
601  shapeToGJK(s, tf, cap);
602  cap->radius = s.radius;
603  cap->height = s.lz / 2;
604 }
605 
606 template <typename S>
607 static void cylToGJK(const Cylinder<S>& s, const Transform3<S>& tf, ccd_cyl_t* cyl)
608 {
609  shapeToGJK(s, tf, cyl);
610  cyl->radius = s.radius;
611  cyl->height = s.lz / 2;
612 }
613 
614 template <typename S>
615 static void coneToGJK(const Cone<S>& s, const Transform3<S>& tf, ccd_cone_t* cone)
616 {
617  shapeToGJK(s, tf, cone);
618  cone->radius = s.radius;
619  cone->height = s.lz / 2;
620 }
621 
622 template <typename S>
623 static void sphereToGJK(const Sphere<S>& s, const Transform3<S>& tf, ccd_sphere_t* sph)
624 {
625  shapeToGJK(s, tf, sph);
626  sph->radius = s.radius;
627 }
628 
629 template <typename S>
630 static void ellipsoidToGJK(const Ellipsoid<S>& s, const Transform3<S>& tf, ccd_ellipsoid_t* ellipsoid)
631 {
632  shapeToGJK(s, tf, ellipsoid);
633  ellipsoid->radii[0] = s.radii[0];
634  ellipsoid->radii[1] = s.radii[1];
635  ellipsoid->radii[2] = s.radii[2];
636 }
637 
638 template <typename S>
639 static void convexToGJK(const Convex<S>& s, const Transform3<S>& tf, ccd_convex_t<S>* conv)
640 {
641  shapeToGJK(s, tf, conv);
642  conv->convex = &s;
643 }
644 
646 static inline void supportBox(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
647 {
648  const ccd_box_t* o = static_cast<const ccd_box_t*>(obj);
649  ccd_vec3_t dir;
650  ccdVec3Copy(&dir, dir_);
651  ccdQuatRotVec(&dir, &o->rot_inv);
652  ccdVec3Set(v, ccdSign(ccdVec3X(&dir)) * o->dim[0],
653  ccdSign(ccdVec3Y(&dir)) * o->dim[1],
654  ccdSign(ccdVec3Z(&dir)) * o->dim[2]);
655  ccdQuatRotVec(v, &o->rot);
656  ccdVec3Add(v, &o->pos);
657 }
658 
659 static inline void supportCap(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
660 {
661  const ccd_cap_t* o = static_cast<const ccd_cap_t*>(obj);
662  ccd_vec3_t dir, pos1, pos2;
663 
664  ccdVec3Copy(&dir, dir_);
665  ccdQuatRotVec(&dir, &o->rot_inv);
666 
667  ccdVec3Set(&pos1, CCD_ZERO, CCD_ZERO, o->height);
668  ccdVec3Set(&pos2, CCD_ZERO, CCD_ZERO, -o->height);
669 
670  ccdVec3Copy(v, &dir);
671  ccdVec3Normalize (v);
672  ccdVec3Scale(v, o->radius);
673  ccdVec3Add(&pos1, v);
674  ccdVec3Add(&pos2, v);
675 
676  if(ccdVec3Z (&dir) > 0)
677  ccdVec3Copy(v, &pos1);
678  else
679  ccdVec3Copy(v, &pos2);
680 
681  // transform support vertex
682  ccdQuatRotVec(v, &o->rot);
683  ccdVec3Add(v, &o->pos);
684 }
685 
686 static inline void supportCyl(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
687 {
688  const ccd_cyl_t* cyl = static_cast<const ccd_cyl_t*>(obj);
689  ccd_vec3_t dir;
690  double zdist, rad;
691 
692  ccdVec3Copy(&dir, dir_);
693  ccdQuatRotVec(&dir, &cyl->rot_inv);
694 
695  zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1];
696  zdist = sqrt(zdist);
697  if(ccdIsZero(zdist))
698  ccdVec3Set(v, 0., 0., ccdSign(ccdVec3Z(&dir)) * cyl->height);
699  else
700  {
701  rad = cyl->radius / zdist;
702 
703  ccdVec3Set(v, rad * ccdVec3X(&dir),
704  rad * ccdVec3Y(&dir),
705  ccdSign(ccdVec3Z(&dir)) * cyl->height);
706  }
707 
708  // transform support vertex
709  ccdQuatRotVec(v, &cyl->rot);
710  ccdVec3Add(v, &cyl->pos);
711 }
712 
713 static inline void supportCone(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
714 {
715  const ccd_cone_t* cone = static_cast<const ccd_cone_t*>(obj);
716  ccd_vec3_t dir;
717 
718  ccdVec3Copy(&dir, dir_);
719  ccdQuatRotVec(&dir, &cone->rot_inv);
720 
721  double zdist, len, rad;
722  zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1];
723  len = zdist + dir.v[2] * dir.v[2];
724  zdist = sqrt(zdist);
725  len = sqrt(len);
726 
727  double sin_a = cone->radius / sqrt(cone->radius * cone->radius + 4 * cone->height * cone->height);
728 
729  if(dir.v[2] > len * sin_a)
730  ccdVec3Set(v, 0., 0., cone->height);
731  else if(zdist > 0)
732  {
733  rad = cone->radius / zdist;
734  ccdVec3Set(v, rad * ccdVec3X(&dir), rad * ccdVec3Y(&dir), -cone->height);
735  }
736  else
737  ccdVec3Set(v, 0, 0, -cone->height);
738 
739  // transform support vertex
740  ccdQuatRotVec(v, &cone->rot);
741  ccdVec3Add(v, &cone->pos);
742 }
743 
744 static inline void supportSphere(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
745 {
746  const ccd_sphere_t* s = static_cast<const ccd_sphere_t*>(obj);
747  ccd_vec3_t dir;
748 
749  ccdVec3Copy(&dir, dir_);
750  ccdQuatRotVec(&dir, &s->rot_inv);
751 
752  ccdVec3Copy(v, &dir);
753  ccdVec3Scale(v, s->radius);
754  ccdVec3Scale(v, CCD_ONE / CCD_SQRT(ccdVec3Len2(&dir)));
755 
756  // transform support vertex
757  ccdQuatRotVec(v, &s->rot);
758  ccdVec3Add(v, &s->pos);
759 }
760 
761 static inline void supportEllipsoid(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
762 {
763  const ccd_ellipsoid_t* s = static_cast<const ccd_ellipsoid_t*>(obj);
764  ccd_vec3_t dir;
765 
766  ccdVec3Copy(&dir, dir_);
767  ccdQuatRotVec(&dir, &s->rot_inv);
768 
769  ccd_vec3_t abc2;
770  abc2.v[0] = s->radii[0] * s->radii[0];
771  abc2.v[1] = s->radii[1] * s->radii[1];
772  abc2.v[2] = s->radii[2] * s->radii[2];
773 
774  v->v[0] = abc2.v[0] * dir.v[0];
775  v->v[1] = abc2.v[1] * dir.v[1];
776  v->v[2] = abc2.v[2] * dir.v[2];
777 
778  ccdVec3Scale(v, CCD_ONE / CCD_SQRT(ccdVec3Dot(v, &dir)));
779 
780  // transform support vertex
781  ccdQuatRotVec(v, &s->rot);
782  ccdVec3Add(v, &s->pos);
783 }
784 
785 template <typename S>
786 static void supportConvex(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
787 {
788  const auto* c = (const ccd_convex_t<S>*)obj;
789  ccd_vec3_t dir, p;
790  ccd_real_t maxdot, dot;
791  int i;
792  Vector3<S>* curp;
793  const auto& center = c->convex->center;
794 
795  ccdVec3Copy(&dir, dir_);
796  ccdQuatRotVec(&dir, &c->rot_inv);
797 
798  maxdot = -CCD_REAL_MAX;
799  curp = c->convex->points;
800 
801  for(i = 0; i < c->convex->num_points; ++i, curp += 1)
802  {
803  ccdVec3Set(&p, (*curp)[0] - center[0], (*curp)[1] - center[1], (*curp)[2] - center[2]);
804  dot = ccdVec3Dot(&dir, &p);
805  if(dot > maxdot)
806  {
807  ccdVec3Set(v, (*curp)[0], (*curp)[1], (*curp)[2]);
808  maxdot = dot;
809  }
810  }
811 
812  // transform support vertex
813  ccdQuatRotVec(v, &c->rot);
814  ccdVec3Add(v, &c->pos);
815 }
816 
817 static void supportTriangle(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
818 {
819  const ccd_triangle_t* tri = static_cast<const ccd_triangle_t*>(obj);
820  ccd_vec3_t dir, p;
821  ccd_real_t maxdot, dot;
822  int i;
823 
824  ccdVec3Copy(&dir, dir_);
825  ccdQuatRotVec(&dir, &tri->rot_inv);
826 
827  maxdot = -CCD_REAL_MAX;
828 
829  for(i = 0; i < 3; ++i)
830  {
831  ccdVec3Set(&p, tri->p[i].v[0] - tri->c.v[0], tri->p[i].v[1] - tri->c.v[1], tri->p[i].v[2] - tri->c.v[2]);
832  dot = ccdVec3Dot(&dir, &p);
833  if(dot > maxdot)
834  {
835  ccdVec3Copy(v, &tri->p[i]);
836  maxdot = dot;
837  }
838  }
839 
840  // transform support vertex
841  ccdQuatRotVec(v, &tri->rot);
842  ccdVec3Add(v, &tri->pos);
843 }
844 
845 static inline void centerShape(const void* obj, ccd_vec3_t* c)
846 {
847  const ccd_obj_t *o = static_cast<const ccd_obj_t*>(obj);
848  ccdVec3Copy(c, &o->pos);
849 }
850 
851 template <typename S>
852 static void centerConvex(const void* obj, ccd_vec3_t* c)
853 {
854  const auto *o = static_cast<const ccd_convex_t<S>*>(obj);
855  ccdVec3Set(c, o->convex->center[0], o->convex->center[1], o->convex->center[2]);
856  ccdQuatRotVec(c, &o->rot);
857  ccdVec3Add(c, &o->pos);
858 }
859 
860 static void centerTriangle(const void* obj, ccd_vec3_t* c)
861 {
862  const ccd_triangle_t *o = static_cast<const ccd_triangle_t*>(obj);
863  ccdVec3Copy(c, &o->c);
864  ccdQuatRotVec(c, &o->rot);
865  ccdVec3Add(c, &o->pos);
866 }
867 
868 template <typename S>
869 bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
870  void* obj2, ccd_support_fn supp2, ccd_center_fn cen2,
871  unsigned int max_iterations, S tolerance,
872  Vector3<S>* contact_points, S* penetration_depth, Vector3<S>* normal)
873 {
874  ccd_t ccd;
875  int res;
876  ccd_real_t depth;
877  ccd_vec3_t dir, pos;
878 
879 
880  CCD_INIT(&ccd);
881  ccd.support1 = supp1;
882  ccd.support2 = supp2;
883  ccd.center1 = cen1;
884  ccd.center2 = cen2;
885  ccd.max_iterations = max_iterations;
886  ccd.mpr_tolerance = tolerance;
887 
888  if(!contact_points)
889  {
890  return ccdMPRIntersect(obj1, obj2, &ccd);
891  }
892 
893 
895  res = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos);
896  if(res == 0)
897  {
898  *contact_points << ccdVec3X(&pos), ccdVec3Y(&pos), ccdVec3Z(&pos);
899  *penetration_depth = depth;
900  *normal << ccdVec3X(&dir), ccdVec3Y(&dir), ccdVec3Z(&dir);
901 
902  return true;
903  }
904 
905  return false;
906 }
907 
908 
910 template <typename S>
911 bool GJKDistance(void* obj1, ccd_support_fn supp1,
912  void* obj2, ccd_support_fn supp2,
913  unsigned int max_iterations, S tolerance,
914  S* res, Vector3<S>* p1, Vector3<S>* p2)
915 {
916  ccd_t ccd;
917  ccd_real_t dist;
918  CCD_INIT(&ccd);
919  ccd.support1 = supp1;
920  ccd.support2 = supp2;
921 
922  ccd.max_iterations = max_iterations;
923  ccd.dist_tolerance = tolerance;
924 
925  ccd_vec3_t p1_, p2_;
926  // NOTE(JS): p1_ and p2_ are set to zeros in order to suppress uninitialized
927  // warning. It seems the warnings occur since libccd_extension::ccdGJKDist2
928  // conditionally set p1_ and p2_. If this wasn't intentional then please
929  // remove the initialization of p1_ and p2_, and change the function
930  // libccd_extension::ccdGJKDist2(...) to always set p1_ and p2_.
931  ccdVec3Set(&p1_, 0.0, 0.0, 0.0);
932  ccdVec3Set(&p2_, 0.0, 0.0, 0.0);
933  dist = libccd_extension::ccdGJKDist2(obj1, obj2, &ccd, &p1_, &p2_);
934  if(p1) *p1 << ccdVec3X(&p1_), ccdVec3Y(&p1_), ccdVec3Z(&p1_);
935  if(p2) *p2 << ccdVec3X(&p2_), ccdVec3Y(&p2_), ccdVec3Z(&p2_);
936  if(res) *res = dist;
937  if(dist < 0) return false;
938  else return true;
939 }
940 
941 template <typename S>
942 GJKSupportFunction GJKInitializer<S, Cylinder<S>>::getSupportFunction()
943 {
944  return &supportCyl;
945 }
946 
947 template <typename S>
948 GJKCenterFunction GJKInitializer<S, Cylinder<S>>::getCenterFunction()
949 {
950  return &centerShape;
951 }
952 
953 template <typename S>
954 void* GJKInitializer<S, Cylinder<S>>::createGJKObject(const Cylinder<S>& s, const Transform3<S>& tf)
955 {
956  ccd_cyl_t* o = new ccd_cyl_t;
957  cylToGJK(s, tf, o);
958  return o;
959 }
960 
961 template <typename S>
962 void GJKInitializer<S, Cylinder<S>>::deleteGJKObject(void* o_)
963 {
964  ccd_cyl_t* o = static_cast<ccd_cyl_t*>(o_);
965  delete o;
966 }
967 
968 template <typename S>
969 GJKSupportFunction GJKInitializer<S, Sphere<S>>::getSupportFunction()
970 {
971  return &supportSphere;
972 }
973 
974 template <typename S>
975 GJKCenterFunction GJKInitializer<S, Sphere<S>>::getCenterFunction()
976 {
977  return &centerShape;
978 }
979 
980 template <typename S>
981 void* GJKInitializer<S, Sphere<S>>::createGJKObject(const Sphere<S>& s, const Transform3<S>& tf)
982 {
983  ccd_sphere_t* o = new ccd_sphere_t;
984  sphereToGJK(s, tf, o);
985  return o;
986 }
987 
988 template <typename S>
989 void GJKInitializer<S, Sphere<S>>::deleteGJKObject(void* o_)
990 {
991  ccd_sphere_t* o = static_cast<ccd_sphere_t*>(o_);
992  delete o;
993 }
994 
995 template <typename S>
996 GJKSupportFunction GJKInitializer<S, Ellipsoid<S>>::getSupportFunction()
997 {
998  return &supportEllipsoid;
999 }
1000 
1001 template <typename S>
1002 GJKCenterFunction GJKInitializer<S, Ellipsoid<S>>::getCenterFunction()
1003 {
1004  return &centerShape;
1005 }
1006 
1007 template <typename S>
1008 void* GJKInitializer<S, Ellipsoid<S>>::createGJKObject(const Ellipsoid<S>& s, const Transform3<S>& tf)
1009 {
1011  ellipsoidToGJK(s, tf, o);
1012  return o;
1013 }
1014 
1015 template <typename S>
1016 void GJKInitializer<S, Ellipsoid<S>>::deleteGJKObject(void* o_)
1017 {
1018  ccd_ellipsoid_t* o = static_cast<ccd_ellipsoid_t*>(o_);
1019  delete o;
1020 }
1021 
1022 template <typename S>
1023 GJKSupportFunction GJKInitializer<S, Box<S>>::getSupportFunction()
1024 {
1025  return &supportBox;
1026 }
1027 
1028 template <typename S>
1029 GJKCenterFunction GJKInitializer<S, Box<S>>::getCenterFunction()
1030 {
1031  return &centerShape;
1032 }
1033 
1034 template <typename S>
1035 void* GJKInitializer<S, Box<S>>::createGJKObject(const Box<S>& s, const Transform3<S>& tf)
1036 {
1037  ccd_box_t* o = new ccd_box_t;
1038  boxToGJK(s, tf, o);
1039  return o;
1040 }
1041 
1042 template <typename S>
1043 void GJKInitializer<S, Box<S>>::deleteGJKObject(void* o_)
1044 {
1045  ccd_box_t* o = static_cast<ccd_box_t*>(o_);
1046  delete o;
1047 }
1048 
1049 template <typename S>
1050 GJKSupportFunction GJKInitializer<S, Capsule<S>>::getSupportFunction()
1051 {
1052  return &supportCap;
1053 }
1054 
1055 template <typename S>
1056 GJKCenterFunction GJKInitializer<S, Capsule<S>>::getCenterFunction()
1057 {
1058  return &centerShape;
1059 }
1060 
1061 template <typename S>
1062 void* GJKInitializer<S, Capsule<S>>::createGJKObject(const Capsule<S>& s, const Transform3<S>& tf)
1063 {
1064  ccd_cap_t* o = new ccd_cap_t;
1065  capToGJK(s, tf, o);
1066  return o;
1067 }
1068 
1069 template <typename S>
1070 void GJKInitializer<S, Capsule<S>>::deleteGJKObject(void* o_)
1071 {
1072  ccd_cap_t* o = static_cast<ccd_cap_t*>(o_);
1073  delete o;
1074 }
1075 
1076 template <typename S>
1077 GJKSupportFunction GJKInitializer<S, Cone<S>>::getSupportFunction()
1078 {
1079  return &supportCone;
1080 }
1081 
1082 template <typename S>
1083 GJKCenterFunction GJKInitializer<S, Cone<S>>::getCenterFunction()
1084 {
1085  return &centerShape;
1086 }
1087 
1088 template <typename S>
1089 void* GJKInitializer<S, Cone<S>>::createGJKObject(const Cone<S>& s, const Transform3<S>& tf)
1090 {
1091  ccd_cone_t* o = new ccd_cone_t;
1092  coneToGJK(s, tf, o);
1093  return o;
1094 }
1095 
1096 template <typename S>
1097 void GJKInitializer<S, Cone<S>>::deleteGJKObject(void* o_)
1098 {
1099  ccd_cone_t* o = static_cast<ccd_cone_t*>(o_);
1100  delete o;
1101 }
1102 
1103 template <typename S>
1104 GJKSupportFunction GJKInitializer<S, Convex<S>>::getSupportFunction()
1105 {
1106  return &supportConvex<S>;
1107 }
1108 
1109 template <typename S>
1110 GJKCenterFunction GJKInitializer<S, Convex<S>>::getCenterFunction()
1111 {
1112  return &centerConvex<S>;
1113 }
1114 
1115 template <typename S>
1116 void* GJKInitializer<S, Convex<S>>::createGJKObject(const Convex<S>& s, const Transform3<S>& tf)
1117 {
1118  auto* o = new ccd_convex_t<S>;
1119  convexToGJK(s, tf, o);
1120  return o;
1121 }
1122 
1123 template <typename S>
1124 void GJKInitializer<S, Convex<S>>::deleteGJKObject(void* o_)
1125 {
1126  auto* o = static_cast<ccd_convex_t<S>*>(o_);
1127  delete o;
1128 }
1129 
1130 inline GJKSupportFunction triGetSupportFunction()
1131 {
1132  return &supportTriangle;
1133 }
1134 
1135 inline GJKCenterFunction triGetCenterFunction()
1136 {
1137  return &centerTriangle;
1138 }
1139 
1140 template <typename S>
1141 void* triCreateGJKObject(const Vector3<S>& P1, const Vector3<S>& P2, const Vector3<S>& P3)
1142 {
1143  ccd_triangle_t* o = new ccd_triangle_t;
1144  Vector3<S> center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3, (P1[2] + P2[2] + P3[2]) / 3);
1145 
1146  ccdVec3Set(&o->p[0], P1[0], P1[1], P1[2]);
1147  ccdVec3Set(&o->p[1], P2[0], P2[1], P2[2]);
1148  ccdVec3Set(&o->p[2], P3[0], P3[1], P3[2]);
1149  ccdVec3Set(&o->c, center[0], center[1], center[2]);
1150  ccdVec3Set(&o->pos, 0., 0., 0.);
1151  ccdQuatSet(&o->rot, 0., 0., 0., 1.);
1152  ccdQuatInvert2(&o->rot_inv, &o->rot);
1153 
1154  return o;
1155 }
1156 
1157 template <typename S>
1158 void* triCreateGJKObject(const Vector3<S>& P1, const Vector3<S>& P2, const Vector3<S>& P3, const Transform3<S>& tf)
1159 {
1160  ccd_triangle_t* o = new ccd_triangle_t;
1161  Vector3<S> center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3, (P1[2] + P2[2] + P3[2]) / 3);
1162 
1163  ccdVec3Set(&o->p[0], P1[0], P1[1], P1[2]);
1164  ccdVec3Set(&o->p[1], P2[0], P2[1], P2[2]);
1165  ccdVec3Set(&o->p[2], P3[0], P3[1], P3[2]);
1166  ccdVec3Set(&o->c, center[0], center[1], center[2]);
1167  const Quaternion<S> q(tf.linear());
1168  const Vector3<S>& T = tf.translation();
1169  ccdVec3Set(&o->pos, T[0], T[1], T[2]);
1170  ccdQuatSet(&o->rot, q.x(), q.y(), q.z(), q.w());
1171  ccdQuatInvert2(&o->rot_inv, &o->rot);
1172 
1173  return o;
1174 }
1175 
1176 inline void triDeleteGJKObject(void* o_)
1177 {
1178  ccd_triangle_t* o = static_cast<ccd_triangle_t*>(o_);
1179  delete o;
1180 }
1181 
1182 } // namespace detail
1183 } // namespace fcl
1184 
1185 #endif
Definition: gjk_libccd-inl.h:144
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
Definition: gjk_libccd-inl.h:160
S lz
Length along z axis.
Definition: capsule.h:61
Definition: simplex.h:28
S radius
Radius of the cylinder.
Definition: cylinder.h:58
Definition: gjk_libccd-inl.h:124
Center at zero point ellipsoid.
Definition: ellipsoid.h:48
S lz
Length along z axis.
Definition: cylinder.h:61
initialize GJK stuffs
Definition: gjk_libccd.h:70
Definition: gjk_libccd-inl.h:118
Center at zero point capsule.
Definition: capsule.h:48
ccd_vec3_t v
Support point in minkowski sum.
Definition: support.h:28
S lz
Length along z axis.
Definition: cone.h:60
Center at zero point, axis aligned box.
Definition: box.h:48
S radius
Radius of the cone.
Definition: cone.h:57
Definition: gjk_libccd-inl.h:139
Vector3< S > side
box side length
Definition: box.h:64
Definition: gjk_libccd-inl.h:155
Definition: gjk_libccd-inl.h:129
ccd_vec3_t v1
Support point in obj1.
Definition: support.h:29
Center at zero cylinder.
Definition: cylinder.h:48
Vector3< S > radii
Radii of the ellipsoid.
Definition: ellipsoid.h:61
Center at zero cone.
Definition: cone.h:48
Definition: support.h:27
ccd_vec3_t v2
Support point in obj2.
Definition: support.h:30
Definition: gjk_libccd-inl.h:134
S radius
Radius of the sphere.
Definition: sphere.h:57
Center at zero point sphere.
Definition: sphere.h:48
Definition: gjk_libccd-inl.h:149
S radius
Radius of capsule.
Definition: capsule.h:58