Bullet Collision Detection & Physics Library
btGjkEpa3.h
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2014 Erwin Coumans http://continuousphysics.com/Bullet/
4 
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the
7 use of this software.
8 Permission is granted to anyone to use this software for any purpose,
9 including commercial applications, and to alter it and redistribute it
10 freely,
11 subject to the following restrictions:
12 
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software in a
15 product, an acknowledgment in the product documentation would be appreciated
16 but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20 */
21 
22 /*
23 Initial GJK-EPA collision solver by Nathanael Presson, 2008
24 Improvements and refactoring by Erwin Coumans, 2008-2014
25 */
26 #ifndef BT_GJK_EPA3_H
27 #define BT_GJK_EPA3_H
28 
29 #include "LinearMath/btTransform.h"
31 
33 {
34  struct sResults
35  {
36  enum eStatus
37  {
38  Separated, /* Shapes doesnt penetrate */
39  Penetrating, /* Shapes are penetrating */
40  GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */
41  EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */
42  } status;
46  };
47 };
48 
49 #if defined(DEBUG) || defined(_DEBUG)
50 #include <stdio.h> //for debug printf
51 #ifdef __SPU__
52 #include <spu_printf.h>
53 #define printf spu_printf
54 #endif //__SPU__
55 #endif
56 
57 // Config
58 
59 /* GJK */
60 #define GJK_MAX_ITERATIONS 128
61 #define GJK_ACCURARY ((btScalar)0.0001)
62 #define GJK_MIN_DISTANCE ((btScalar)0.0001)
63 #define GJK_DUPLICATED_EPS ((btScalar)0.0001)
64 #define GJK_SIMPLEX2_EPS ((btScalar)0.0)
65 #define GJK_SIMPLEX3_EPS ((btScalar)0.0)
66 #define GJK_SIMPLEX4_EPS ((btScalar)0.0)
67 
68 /* EPA */
69 #define EPA_MAX_VERTICES 64
70 #define EPA_MAX_FACES (EPA_MAX_VERTICES * 2)
71 #define EPA_MAX_ITERATIONS 255
72 #define EPA_ACCURACY ((btScalar)0.0001)
73 #define EPA_FALLBACK (10 * EPA_ACCURACY)
74 #define EPA_PLANE_EPS ((btScalar)0.00001)
75 #define EPA_INSIDE_EPS ((btScalar)0.01)
76 
77 // Shorthands
78 typedef unsigned int U;
79 typedef unsigned char U1;
80 
81 // MinkowskiDiff
82 template <typename btConvexTemplate>
84 {
85  const btConvexTemplate* m_convexAPtr;
86  const btConvexTemplate* m_convexBPtr;
87 
90 
92 
93  MinkowskiDiff(const btConvexTemplate& a, const btConvexTemplate& b)
94  : m_convexAPtr(&a),
95  m_convexBPtr(&b)
96  {
97  }
98 
99  void EnableMargin(bool enable)
100  {
101  m_enableMargin = enable;
102  }
103  inline btVector3 Support0(const btVector3& d) const
104  {
105  return m_convexAPtr->getLocalSupportWithMargin(d);
106  }
107  inline btVector3 Support1(const btVector3& d) const
108  {
109  return m_toshape0 * m_convexBPtr->getLocalSupportWithMargin(m_toshape1 * d);
110  }
111 
112  inline btVector3 Support(const btVector3& d) const
113  {
114  return (Support0(d) - Support1(-d));
115  }
116  btVector3 Support(const btVector3& d, U index) const
117  {
118  if (index)
119  return (Support1(d));
120  else
121  return (Support0(d));
122  }
123 };
124 
126 {
130 };
131 
132 // GJK
133 template <typename btConvexTemplate>
134 struct GJK
135 {
136  /* Types */
137  struct sSV
138  {
140  };
141  struct sSimplex
142  {
143  sSV* c[4];
146  };
147 
148  /* Fields */
149 
155  sSV* m_free[4];
160  /* Methods */
161 
162  GJK(const btConvexTemplate& a, const btConvexTemplate& b)
163  : m_shape(a, b)
164  {
165  Initialize();
166  }
167  void Initialize()
168  {
169  m_ray = btVector3(0, 0, 0);
170  m_nfree = 0;
172  m_current = 0;
173  m_distance = 0;
174  }
176  {
177  U iterations = 0;
178  btScalar sqdist = 0;
179  btScalar alpha = 0;
180  btVector3 lastw[4];
181  U clastw = 0;
182  /* Initialize solver */
183  m_free[0] = &m_store[0];
184  m_free[1] = &m_store[1];
185  m_free[2] = &m_store[2];
186  m_free[3] = &m_store[3];
187  m_nfree = 4;
188  m_current = 0;
190  m_shape = shapearg;
191  m_distance = 0;
192  /* Initialize simplex */
193  m_simplices[0].rank = 0;
194  m_ray = guess;
195  const btScalar sqrl = m_ray.length2();
196  appendvertice(m_simplices[0], sqrl > 0 ? -m_ray : btVector3(1, 0, 0));
197  m_simplices[0].p[0] = 1;
198  m_ray = m_simplices[0].c[0]->w;
199  sqdist = sqrl;
200  lastw[0] =
201  lastw[1] =
202  lastw[2] =
203  lastw[3] = m_ray;
204  /* Loop */
205  do
206  {
207  const U next = 1 - m_current;
209  sSimplex& ns = m_simplices[next];
210  /* Check zero */
211  const btScalar rl = m_ray.length();
212  if (rl < GJK_MIN_DISTANCE)
213  { /* Touching or inside */
215  break;
216  }
217  /* Append new vertice in -'v' direction */
218  appendvertice(cs, -m_ray);
219  const btVector3& w = cs.c[cs.rank - 1]->w;
220  bool found = false;
221  for (U i = 0; i < 4; ++i)
222  {
223  if ((w - lastw[i]).length2() < GJK_DUPLICATED_EPS)
224  {
225  found = true;
226  break;
227  }
228  }
229  if (found)
230  { /* Return old simplex */
232  break;
233  }
234  else
235  { /* Update lastw */
236  lastw[clastw = (clastw + 1) & 3] = w;
237  }
238  /* Check for termination */
239  const btScalar omega = btDot(m_ray, w) / rl;
240  alpha = btMax(omega, alpha);
241  if (((rl - alpha) - (GJK_ACCURARY * rl)) <= 0)
242  { /* Return old simplex */
244  break;
245  }
246  /* Reduce simplex */
247  btScalar weights[4];
248  U mask = 0;
249  switch (cs.rank)
250  {
251  case 2:
252  sqdist = projectorigin(cs.c[0]->w,
253  cs.c[1]->w,
254  weights, mask);
255  break;
256  case 3:
257  sqdist = projectorigin(cs.c[0]->w,
258  cs.c[1]->w,
259  cs.c[2]->w,
260  weights, mask);
261  break;
262  case 4:
263  sqdist = projectorigin(cs.c[0]->w,
264  cs.c[1]->w,
265  cs.c[2]->w,
266  cs.c[3]->w,
267  weights, mask);
268  break;
269  }
270  if (sqdist >= 0)
271  { /* Valid */
272  ns.rank = 0;
273  m_ray = btVector3(0, 0, 0);
274  m_current = next;
275  for (U i = 0, ni = cs.rank; i < ni; ++i)
276  {
277  if (mask & (1 << i))
278  {
279  ns.c[ns.rank] = cs.c[i];
280  ns.p[ns.rank++] = weights[i];
281  m_ray += cs.c[i]->w * weights[i];
282  }
283  else
284  {
285  m_free[m_nfree++] = cs.c[i];
286  }
287  }
288  if (mask == 15) m_status = eGjkInside;
289  }
290  else
291  { /* Return old simplex */
293  break;
294  }
295  m_status = ((++iterations) < GJK_MAX_ITERATIONS) ? m_status : eGjkFailed;
296  } while (m_status == eGjkValid);
298  switch (m_status)
299  {
300  case eGjkValid:
301  m_distance = m_ray.length();
302  break;
303  case eGjkInside:
304  m_distance = 0;
305  break;
306  default:
307  {
308  }
309  }
310  return (m_status);
311  }
313  {
314  switch (m_simplex->rank)
315  {
316  case 1:
317  {
318  for (U i = 0; i < 3; ++i)
319  {
320  btVector3 axis = btVector3(0, 0, 0);
321  axis[i] = 1;
322  appendvertice(*m_simplex, axis);
323  if (EncloseOrigin()) return (true);
325  appendvertice(*m_simplex, -axis);
326  if (EncloseOrigin()) return (true);
328  }
329  }
330  break;
331  case 2:
332  {
333  const btVector3 d = m_simplex->c[1]->w - m_simplex->c[0]->w;
334  for (U i = 0; i < 3; ++i)
335  {
336  btVector3 axis = btVector3(0, 0, 0);
337  axis[i] = 1;
338  const btVector3 p = btCross(d, axis);
339  if (p.length2() > 0)
340  {
342  if (EncloseOrigin()) return (true);
344  appendvertice(*m_simplex, -p);
345  if (EncloseOrigin()) return (true);
347  }
348  }
349  }
350  break;
351  case 3:
352  {
353  const btVector3 n = btCross(m_simplex->c[1]->w - m_simplex->c[0]->w,
354  m_simplex->c[2]->w - m_simplex->c[0]->w);
355  if (n.length2() > 0)
356  {
358  if (EncloseOrigin()) return (true);
360  appendvertice(*m_simplex, -n);
361  if (EncloseOrigin()) return (true);
363  }
364  }
365  break;
366  case 4:
367  {
368  if (btFabs(det(m_simplex->c[0]->w - m_simplex->c[3]->w,
369  m_simplex->c[1]->w - m_simplex->c[3]->w,
370  m_simplex->c[2]->w - m_simplex->c[3]->w)) > 0)
371  return (true);
372  }
373  break;
374  }
375  return (false);
376  }
377  /* Internals */
378  void getsupport(const btVector3& d, sSV& sv) const
379  {
380  sv.d = d / d.length();
381  sv.w = m_shape.Support(sv.d);
382  }
383  void removevertice(sSimplex& simplex)
384  {
385  m_free[m_nfree++] = simplex.c[--simplex.rank];
386  }
387  void appendvertice(sSimplex& simplex, const btVector3& v)
388  {
389  simplex.p[simplex.rank] = 0;
390  simplex.c[simplex.rank] = m_free[--m_nfree];
391  getsupport(v, *simplex.c[simplex.rank++]);
392  }
393  static btScalar det(const btVector3& a, const btVector3& b, const btVector3& c)
394  {
395  return (a.y() * b.z() * c.x() + a.z() * b.x() * c.y() -
396  a.x() * b.z() * c.y() - a.y() * b.x() * c.z() +
397  a.x() * b.y() * c.z() - a.z() * b.y() * c.x());
398  }
400  const btVector3& b,
401  btScalar* w, U& m)
402  {
403  const btVector3 d = b - a;
404  const btScalar l = d.length2();
405  if (l > GJK_SIMPLEX2_EPS)
406  {
407  const btScalar t(l > 0 ? -btDot(a, d) / l : 0);
408  if (t >= 1)
409  {
410  w[0] = 0;
411  w[1] = 1;
412  m = 2;
413  return (b.length2());
414  }
415  else if (t <= 0)
416  {
417  w[0] = 1;
418  w[1] = 0;
419  m = 1;
420  return (a.length2());
421  }
422  else
423  {
424  w[0] = 1 - (w[1] = t);
425  m = 3;
426  return ((a + d * t).length2());
427  }
428  }
429  return (-1);
430  }
432  const btVector3& b,
433  const btVector3& c,
434  btScalar* w, U& m)
435  {
436  static const U imd3[] = {1, 2, 0};
437  const btVector3* vt[] = {&a, &b, &c};
438  const btVector3 dl[] = {a - b, b - c, c - a};
439  const btVector3 n = btCross(dl[0], dl[1]);
440  const btScalar l = n.length2();
441  if (l > GJK_SIMPLEX3_EPS)
442  {
443  btScalar mindist = -1;
444  btScalar subw[2] = {0.f, 0.f};
445  U subm(0);
446  for (U i = 0; i < 3; ++i)
447  {
448  if (btDot(*vt[i], btCross(dl[i], n)) > 0)
449  {
450  const U j = imd3[i];
451  const btScalar subd(projectorigin(*vt[i], *vt[j], subw, subm));
452  if ((mindist < 0) || (subd < mindist))
453  {
454  mindist = subd;
455  m = static_cast<U>(((subm & 1) ? 1 << i : 0) + ((subm & 2) ? 1 << j : 0));
456  w[i] = subw[0];
457  w[j] = subw[1];
458  w[imd3[j]] = 0;
459  }
460  }
461  }
462  if (mindist < 0)
463  {
464  const btScalar d = btDot(a, n);
465  const btScalar s = btSqrt(l);
466  const btVector3 p = n * (d / l);
467  mindist = p.length2();
468  m = 7;
469  w[0] = (btCross(dl[1], b - p)).length() / s;
470  w[1] = (btCross(dl[2], c - p)).length() / s;
471  w[2] = 1 - (w[0] + w[1]);
472  }
473  return (mindist);
474  }
475  return (-1);
476  }
478  const btVector3& b,
479  const btVector3& c,
480  const btVector3& d,
481  btScalar* w, U& m)
482  {
483  static const U imd3[] = {1, 2, 0};
484  const btVector3* vt[] = {&a, &b, &c, &d};
485  const btVector3 dl[] = {a - d, b - d, c - d};
486  const btScalar vl = det(dl[0], dl[1], dl[2]);
487  const bool ng = (vl * btDot(a, btCross(b - c, a - b))) <= 0;
488  if (ng && (btFabs(vl) > GJK_SIMPLEX4_EPS))
489  {
490  btScalar mindist = -1;
491  btScalar subw[3] = {0.f, 0.f, 0.f};
492  U subm(0);
493  for (U i = 0; i < 3; ++i)
494  {
495  const U j = imd3[i];
496  const btScalar s = vl * btDot(d, btCross(dl[i], dl[j]));
497  if (s > 0)
498  {
499  const btScalar subd = projectorigin(*vt[i], *vt[j], d, subw, subm);
500  if ((mindist < 0) || (subd < mindist))
501  {
502  mindist = subd;
503  m = static_cast<U>((subm & 1 ? 1 << i : 0) +
504  (subm & 2 ? 1 << j : 0) +
505  (subm & 4 ? 8 : 0));
506  w[i] = subw[0];
507  w[j] = subw[1];
508  w[imd3[j]] = 0;
509  w[3] = subw[2];
510  }
511  }
512  }
513  if (mindist < 0)
514  {
515  mindist = 0;
516  m = 15;
517  w[0] = det(c, b, d) / vl;
518  w[1] = det(a, c, d) / vl;
519  w[2] = det(b, a, d) / vl;
520  w[3] = 1 - (w[0] + w[1] + w[2]);
521  }
522  return (mindist);
523  }
524  return (-1);
525  }
526 };
527 
529 {
540 };
541 
542 // EPA
543 template <typename btConvexTemplate>
544 struct EPA
545 {
546  /* Types */
547 
548  struct sFace
549  {
553  sFace* f[3];
554  sFace* l[2];
555  U1 e[3];
557  };
558  struct sList
559  {
562  sList() : root(0), count(0) {}
563  };
564  struct sHorizon
565  {
568  U nf;
569  sHorizon() : cf(0), ff(0), nf(0) {}
570  };
571 
572  /* Fields */
582  /* Methods */
583  EPA()
584  {
585  Initialize();
586  }
587 
588  static inline void bind(sFace* fa, U ea, sFace* fb, U eb)
589  {
590  fa->e[ea] = (U1)eb;
591  fa->f[ea] = fb;
592  fb->e[eb] = (U1)ea;
593  fb->f[eb] = fa;
594  }
595  static inline void append(sList& list, sFace* face)
596  {
597  face->l[0] = 0;
598  face->l[1] = list.root;
599  if (list.root) list.root->l[0] = face;
600  list.root = face;
601  ++list.count;
602  }
603  static inline void remove(sList& list, sFace* face)
604  {
605  if (face->l[1]) face->l[1]->l[0] = face->l[0];
606  if (face->l[0]) face->l[0]->l[1] = face->l[1];
607  if (face == list.root) list.root = face->l[1];
608  --list.count;
609  }
610 
611  void Initialize()
612  {
614  m_normal = btVector3(0, 0, 0);
615  m_depth = 0;
616  m_nextsv = 0;
617  for (U i = 0; i < EPA_MAX_FACES; ++i)
618  {
620  }
621  }
623  {
624  typename GJK<btConvexTemplate>::sSimplex& simplex = *gjk.m_simplex;
625  if ((simplex.rank > 1) && gjk.EncloseOrigin())
626  {
627  /* Clean up */
628  while (m_hull.root)
629  {
630  sFace* f = m_hull.root;
631  remove(m_hull, f);
632  append(m_stock, f);
633  }
635  m_nextsv = 0;
636  /* Orient simplex */
637  if (gjk.det(simplex.c[0]->w - simplex.c[3]->w,
638  simplex.c[1]->w - simplex.c[3]->w,
639  simplex.c[2]->w - simplex.c[3]->w) < 0)
640  {
641  btSwap(simplex.c[0], simplex.c[1]);
642  btSwap(simplex.p[0], simplex.p[1]);
643  }
644  /* Build initial hull */
645  sFace* tetra[] = {newface(simplex.c[0], simplex.c[1], simplex.c[2], true),
646  newface(simplex.c[1], simplex.c[0], simplex.c[3], true),
647  newface(simplex.c[2], simplex.c[1], simplex.c[3], true),
648  newface(simplex.c[0], simplex.c[2], simplex.c[3], true)};
649  if (m_hull.count == 4)
650  {
651  sFace* best = findbest();
652  sFace outer = *best;
653  U pass = 0;
654  U iterations = 0;
655  bind(tetra[0], 0, tetra[1], 0);
656  bind(tetra[0], 1, tetra[2], 0);
657  bind(tetra[0], 2, tetra[3], 0);
658  bind(tetra[1], 1, tetra[3], 2);
659  bind(tetra[1], 2, tetra[2], 1);
660  bind(tetra[2], 2, tetra[3], 1);
662  for (; iterations < EPA_MAX_ITERATIONS; ++iterations)
663  {
665  {
666  sHorizon horizon;
668  bool valid = true;
669  best->pass = (U1)(++pass);
670  gjk.getsupport(best->n, *w);
671  const btScalar wdist = btDot(best->n, w->w) - best->d;
672  if (wdist > EPA_ACCURACY)
673  {
674  for (U j = 0; (j < 3) && valid; ++j)
675  {
676  valid &= expand(pass, w,
677  best->f[j], best->e[j],
678  horizon);
679  }
680  if (valid && (horizon.nf >= 3))
681  {
682  bind(horizon.cf, 1, horizon.ff, 2);
683  remove(m_hull, best);
684  append(m_stock, best);
685  best = findbest();
686  outer = *best;
687  }
688  else
689  {
691  break;
692  }
693  }
694  else
695  {
697  break;
698  }
699  }
700  else
701  {
703  break;
704  }
705  }
706  const btVector3 projection = outer.n * outer.d;
707  m_normal = outer.n;
708  m_depth = outer.d;
709  m_result.rank = 3;
710  m_result.c[0] = outer.c[0];
711  m_result.c[1] = outer.c[1];
712  m_result.c[2] = outer.c[2];
713  m_result.p[0] = btCross(outer.c[1]->w - projection,
714  outer.c[2]->w - projection)
715  .length();
716  m_result.p[1] = btCross(outer.c[2]->w - projection,
717  outer.c[0]->w - projection)
718  .length();
719  m_result.p[2] = btCross(outer.c[0]->w - projection,
720  outer.c[1]->w - projection)
721  .length();
722  const btScalar sum = m_result.p[0] + m_result.p[1] + m_result.p[2];
723  m_result.p[0] /= sum;
724  m_result.p[1] /= sum;
725  m_result.p[2] /= sum;
726  return (m_status);
727  }
728  }
729  /* Fallback */
731  m_normal = -guess;
732  const btScalar nl = m_normal.length();
733  if (nl > 0)
734  m_normal = m_normal / nl;
735  else
736  m_normal = btVector3(1, 0, 0);
737  m_depth = 0;
738  m_result.rank = 1;
739  m_result.c[0] = simplex.c[0];
740  m_result.p[0] = 1;
741  return (m_status);
742  }
744  {
745  const btVector3 ba = b->w - a->w;
746  const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane
747  const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required
748 
749  if (a_dot_nab < 0)
750  {
751  // Outside of edge a->b
752 
753  const btScalar ba_l2 = ba.length2();
754  const btScalar a_dot_ba = btDot(a->w, ba);
755  const btScalar b_dot_ba = btDot(b->w, ba);
756 
757  if (a_dot_ba > 0)
758  {
759  // Pick distance vertex a
760  dist = a->w.length();
761  }
762  else if (b_dot_ba < 0)
763  {
764  // Pick distance vertex b
765  dist = b->w.length();
766  }
767  else
768  {
769  // Pick distance to edge a->b
770  const btScalar a_dot_b = btDot(a->w, b->w);
771  dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0));
772  }
773 
774  return true;
775  }
776 
777  return false;
778  }
780  {
781  if (m_stock.root)
782  {
783  sFace* face = m_stock.root;
784  remove(m_stock, face);
785  append(m_hull, face);
786  face->pass = 0;
787  face->c[0] = a;
788  face->c[1] = b;
789  face->c[2] = c;
790  face->n = btCross(b->w - a->w, c->w - a->w);
791  const btScalar l = face->n.length();
792  const bool v = l > EPA_ACCURACY;
793 
794  if (v)
795  {
796  if (!(getedgedist(face, a, b, face->d) ||
797  getedgedist(face, b, c, face->d) ||
798  getedgedist(face, c, a, face->d)))
799  {
800  // Origin projects to the interior of the triangle
801  // Use distance to triangle plane
802  face->d = btDot(a->w, face->n) / l;
803  }
804 
805  face->n /= l;
806  if (forced || (face->d >= -EPA_PLANE_EPS))
807  {
808  return face;
809  }
810  else
812  }
813  else
815 
816  remove(m_hull, face);
817  append(m_stock, face);
818  return 0;
819  }
821  return 0;
822  }
824  {
825  sFace* minf = m_hull.root;
826  btScalar mind = minf->d * minf->d;
827  for (sFace* f = minf->l[1]; f; f = f->l[1])
828  {
829  const btScalar sqd = f->d * f->d;
830  if (sqd < mind)
831  {
832  minf = f;
833  mind = sqd;
834  }
835  }
836  return (minf);
837  }
838  bool expand(U pass, typename GJK<btConvexTemplate>::sSV* w, sFace* f, U e, sHorizon& horizon)
839  {
840  static const U i1m3[] = {1, 2, 0};
841  static const U i2m3[] = {2, 0, 1};
842  if (f->pass != pass)
843  {
844  const U e1 = i1m3[e];
845  if ((btDot(f->n, w->w) - f->d) < -EPA_PLANE_EPS)
846  {
847  sFace* nf = newface(f->c[e1], f->c[e], w, false);
848  if (nf)
849  {
850  bind(nf, 0, f, e);
851  if (horizon.cf)
852  bind(horizon.cf, 1, nf, 2);
853  else
854  horizon.ff = nf;
855  horizon.cf = nf;
856  ++horizon.nf;
857  return (true);
858  }
859  }
860  else
861  {
862  const U e2 = i2m3[e];
863  f->pass = (U1)pass;
864  if (expand(pass, w, f->f[e1], f->e[e1], horizon) &&
865  expand(pass, w, f->f[e2], f->e[e2], horizon))
866  {
867  remove(m_hull, f);
868  append(m_stock, f);
869  return (true);
870  }
871  }
872  }
873  return (false);
874  }
875 };
876 
877 template <typename btConvexTemplate>
878 static void Initialize(const btConvexTemplate& a, const btConvexTemplate& b,
879  btGjkEpaSolver3::sResults& results,
881 {
882  /* Results */
883  results.witnesses[0] =
884  results.witnesses[1] = btVector3(0, 0, 0);
886  /* Shape */
887 
888  shape.m_toshape1 = b.getWorldTransform().getBasis().transposeTimes(a.getWorldTransform().getBasis());
889  shape.m_toshape0 = a.getWorldTransform().inverseTimes(b.getWorldTransform());
890 }
891 
892 //
893 // Api
894 //
895 
896 //
897 template <typename btConvexTemplate>
898 bool btGjkEpaSolver3_Distance(const btConvexTemplate& a, const btConvexTemplate& b,
899  const btVector3& guess,
900  btGjkEpaSolver3::sResults& results)
901 {
903  Initialize(a, b, results, shape);
904  GJK<btConvexTemplate> gjk(a, b);
905  eGjkStatus gjk_status = gjk.Evaluate(shape, guess);
906  if (gjk_status == eGjkValid)
907  {
908  btVector3 w0 = btVector3(0, 0, 0);
909  btVector3 w1 = btVector3(0, 0, 0);
910  for (U i = 0; i < gjk.m_simplex->rank; ++i)
911  {
912  const btScalar p = gjk.m_simplex->p[i];
913  w0 += shape.Support(gjk.m_simplex->c[i]->d, 0) * p;
914  w1 += shape.Support(-gjk.m_simplex->c[i]->d, 1) * p;
915  }
916  results.witnesses[0] = a.getWorldTransform() * w0;
917  results.witnesses[1] = a.getWorldTransform() * w1;
918  results.normal = w0 - w1;
919  results.distance = results.normal.length();
920  results.normal /= results.distance > GJK_MIN_DISTANCE ? results.distance : 1;
921  return (true);
922  }
923  else
924  {
926  return (false);
927  }
928 }
929 
930 template <typename btConvexTemplate>
931 bool btGjkEpaSolver3_Penetration(const btConvexTemplate& a,
932  const btConvexTemplate& b,
933  const btVector3& guess,
934  btGjkEpaSolver3::sResults& results)
935 {
937  Initialize(a, b, results, shape);
938  GJK<btConvexTemplate> gjk(a, b);
939  eGjkStatus gjk_status = gjk.Evaluate(shape, -guess);
940  switch (gjk_status)
941  {
942  case eGjkInside:
943  {
945  eEpaStatus epa_status = epa.Evaluate(gjk, -guess);
946  if (epa_status != eEpaFailed)
947  {
948  btVector3 w0 = btVector3(0, 0, 0);
949  for (U i = 0; i < epa.m_result.rank; ++i)
950  {
951  w0 += shape.Support(epa.m_result.c[i]->d, 0) * epa.m_result.p[i];
952  }
954  results.witnesses[0] = a.getWorldTransform() * w0;
955  results.witnesses[1] = a.getWorldTransform() * (w0 - epa.m_normal * epa.m_depth);
956  results.normal = -epa.m_normal;
957  results.distance = -epa.m_depth;
958  return (true);
959  }
960  else
962  }
963  break;
964  case eGjkFailed:
966  break;
967  default:
968  {
969  }
970  }
971  return (false);
972 }
973 
974 #if 0
975 int btComputeGjkEpaPenetration2(const btCollisionDescription& colDesc, btDistanceInfo* distInfo)
976 {
978  btVector3 guess = colDesc.m_firstDir;
979 
980  bool res = btGjkEpaSolver3::Penetration(colDesc.m_objA,colDesc.m_objB,
981  colDesc.m_transformA,colDesc.m_transformB,
982  colDesc.m_localSupportFuncA,colDesc.m_localSupportFuncB,
983  guess,
984  results);
985  if (res)
986  {
987  if ((results.status==btGjkEpaSolver3::sResults::Penetrating) || results.status==GJK::eStatus::Inside)
988  {
989  //normal could be 'swapped'
990 
991  distInfo->m_distance = results.distance;
992  distInfo->m_normalBtoA = results.normal;
993  btVector3 tmpNormalInB = results.witnesses[1]-results.witnesses[0];
994  btScalar lenSqr = tmpNormalInB.length2();
995  if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
996  {
997  tmpNormalInB = results.normal;
998  lenSqr = results.normal.length2();
999  }
1000 
1001  if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
1002  {
1003  tmpNormalInB /= btSqrt(lenSqr);
1004  btScalar distance2 = -(results.witnesses[0]-results.witnesses[1]).length();
1005  //only replace valid penetrations when the result is deeper (check)
1006  //if ((distance2 < results.distance))
1007  {
1008  distInfo->m_distance = distance2;
1009  distInfo->m_pointOnA= results.witnesses[0];
1010  distInfo->m_pointOnB= results.witnesses[1];
1011  distInfo->m_normalBtoA= tmpNormalInB;
1012  return 0;
1013  }
1014  }
1015  }
1016 
1017  }
1018 
1019  return -1;
1020 }
1021 #endif
1022 
1023 template <typename btConvexTemplate, typename btDistanceInfoTemplate>
1024 int btComputeGjkDistance(const btConvexTemplate& a, const btConvexTemplate& b,
1025  const btGjkCollisionDescription& colDesc, btDistanceInfoTemplate* distInfo)
1026 {
1027  btGjkEpaSolver3::sResults results;
1028  btVector3 guess = colDesc.m_firstDir;
1029 
1030  bool isSeparated = btGjkEpaSolver3_Distance(a, b,
1031  guess,
1032  results);
1033  if (isSeparated)
1034  {
1035  distInfo->m_distance = results.distance;
1036  distInfo->m_pointOnA = results.witnesses[0];
1037  distInfo->m_pointOnB = results.witnesses[1];
1038  distInfo->m_normalBtoA = results.normal;
1039  return 0;
1040  }
1041 
1042  return -1;
1043 }
1044 
1045 /* Symbols cleanup */
1046 
1047 #undef GJK_MAX_ITERATIONS
1048 #undef GJK_ACCURARY
1049 #undef GJK_MIN_DISTANCE
1050 #undef GJK_DUPLICATED_EPS
1051 #undef GJK_SIMPLEX2_EPS
1052 #undef GJK_SIMPLEX3_EPS
1053 #undef GJK_SIMPLEX4_EPS
1054 
1055 #undef EPA_MAX_VERTICES
1056 #undef EPA_MAX_FACES
1057 #undef EPA_MAX_ITERATIONS
1058 #undef EPA_ACCURACY
1059 #undef EPA_FALLBACK
1060 #undef EPA_PLANE_EPS
1061 #undef EPA_INSIDE_EPS
1062 
1063 #endif //BT_GJK_EPA3_H
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btGjkEpaSolver3::sResults::GJK_Failed
Definition: btGjkEpa3.h:40
eEpaNonConvex
Definition: btGjkEpa3.h:533
EPA::sList::sList
sList()
Definition: btGjkEpa3.h:562
eGjkFailed
Definition: btGjkEpa3.h:129
eEpaFailed
Definition: btGjkEpa3.h:539
EPA_MAX_ITERATIONS
#define EPA_MAX_ITERATIONS
Definition: btGjkEpa3.h:71
U
unsigned int U
Definition: btGjkEpa3.h:78
btComputeGjkDistance
int btComputeGjkDistance(const btConvexTemplate &a, const btConvexTemplate &b, const btGjkCollisionDescription &colDesc, btDistanceInfoTemplate *distInfo)
Definition: btGjkEpa3.h:1024
EPA::m_status
eEpaStatus m_status
Definition: btGjkEpa3.h:573
MinkowskiDiff::Support
btVector3 Support(const btVector3 &d) const
Definition: btGjkEpa3.h:112
MinkowskiDiff::m_toshape1
btMatrix3x3 m_toshape1
Definition: btGjkEpa3.h:88
GJK::sSimplex::p
btScalar p[4]
Definition: btGjkEpa3.h:144
GJK_SIMPLEX4_EPS
#define GJK_SIMPLEX4_EPS
Definition: btGjkEpa3.h:66
MinkowskiDiff
Definition: btGjkEpa3.h:83
GJK::projectorigin
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, btScalar *w, U &m)
Definition: btGjkEpa3.h:399
MinkowskiDiff::Support1
btVector3 Support1(const btVector3 &d) const
Definition: btGjkEpa3.h:107
eEpaTouching
Definition: btGjkEpa3.h:531
GJK::Evaluate
eGjkStatus Evaluate(const MinkowskiDiff< btConvexTemplate > &shapearg, const btVector3 &guess)
Definition: btGjkEpa3.h:175
eEpaOutOfFaces
Definition: btGjkEpa3.h:535
eEpaStatus
eEpaStatus
Definition: btGjkEpa3.h:528
GJK::m_ray
btVector3 m_ray
Definition: btGjkEpa3.h:151
MinkowskiDiff::m_convexBPtr
const btConvexTemplate * m_convexBPtr
Definition: btGjkEpa3.h:86
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257
EPA::sHorizon
Definition: btGjkEpa3.h:564
GJK::getsupport
void getsupport(const btVector3 &d, sSV &sv) const
Definition: btGjkEpa3.h:378
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
EPA::sFace::pass
U1 pass
Definition: btGjkEpa3.h:556
EPA::sList::root
sFace * root
Definition: btGjkEpa3.h:560
EPA_PLANE_EPS
#define EPA_PLANE_EPS
Definition: btGjkEpa3.h:74
MinkowskiDiff::EnableMargin
void EnableMargin(bool enable)
Definition: btGjkEpa3.h:99
GJK::m_distance
btScalar m_distance
Definition: btGjkEpa3.h:152
MinkowskiDiff::m_enableMargin
bool m_enableMargin
Definition: btGjkEpa3.h:91
EPA::sFace::f
sFace * f[3]
Definition: btGjkEpa3.h:553
GJK::m_simplices
sSimplex m_simplices[2]
Definition: btGjkEpa3.h:153
btGjkCollisionDescription
Definition: btGjkCollisionDescription.h:21
eGjkValid
Definition: btGjkEpa3.h:127
EPA::m_stock
sList m_stock
Definition: btGjkEpa3.h:581
EPA
Definition: btGjkEpa3.h:544
EPA::sFace
Definition: btGjkEpa3.h:548
btGjkEpaSolver3::sResults::distance
btScalar distance
Definition: btGjkEpa3.h:45
btCross
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:918
GJK_DUPLICATED_EPS
#define GJK_DUPLICATED_EPS
Definition: btGjkEpa3.h:63
EPA::sFace::c
GJK< btConvexTemplate >::sSV * c[3]
Definition: btGjkEpa3.h:552
GJK_SIMPLEX2_EPS
#define GJK_SIMPLEX2_EPS
Definition: btGjkEpa3.h:64
GJK::m_status
eGjkStatus m_status
Definition: btGjkEpa3.h:159
EPA_MAX_VERTICES
#define EPA_MAX_VERTICES
Definition: btGjkEpa3.h:69
GJK::m_store
sSV m_store[4]
Definition: btGjkEpa3.h:154
btGjkEpaSolver3::sResults
Definition: btGjkEpa3.h:34
GJK
Definition: btGjkEpa3.h:134
btMax
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
EPA::Initialize
void Initialize()
Definition: btGjkEpa3.h:611
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
EPA::sFace::e
U1 e[3]
Definition: btGjkEpa3.h:555
btGjkCollisionDescription::m_firstDir
btVector3 m_firstDir
Definition: btGjkCollisionDescription.h:23
btGjkEpaSolver3::sResults::normal
btVector3 normal
Definition: btGjkEpa3.h:44
Initialize
static void Initialize(const btConvexTemplate &a, const btConvexTemplate &b, btGjkEpaSolver3::sResults &results, MinkowskiDiff< btConvexTemplate > &shape)
Definition: btGjkEpa3.h:878
GJK::sSimplex
Definition: btGjkEpa3.h:141
EPA::bind
static void bind(sFace *fa, U ea, sFace *fb, U eb)
Definition: btGjkEpa3.h:588
EPA::sHorizon::sHorizon
sHorizon()
Definition: btGjkEpa3.h:569
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:497
EPA::remove
static void remove(sList &list, sFace *face)
Definition: btGjkEpa3.h:603
EPA_MAX_FACES
#define EPA_MAX_FACES
Definition: btGjkEpa3.h:70
MinkowskiDiff::Support0
btVector3 Support0(const btVector3 &d) const
Definition: btGjkEpa3.h:103
MinkowskiDiff::MinkowskiDiff
MinkowskiDiff(const btConvexTemplate &a, const btConvexTemplate &b)
Definition: btGjkEpa3.h:93
EPA::getedgedist
bool getedgedist(sFace *face, typename GJK< btConvexTemplate >::sSV *a, typename GJK< btConvexTemplate >::sSV *b, btScalar &dist)
Definition: btGjkEpa3.h:743
btGjkCollisionDescription.h
EPA::newface
sFace * newface(typename GJK< btConvexTemplate >::sSV *a, typename GJK< btConvexTemplate >::sSV *b, typename GJK< btConvexTemplate >::sSV *c, bool forced)
Definition: btGjkEpa3.h:779
GJK::m_simplex
sSimplex * m_simplex
Definition: btGjkEpa3.h:158
GJK::projectorigin
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, btScalar *w, U &m)
Definition: btGjkEpa3.h:431
eGjkStatus
eGjkStatus
Definition: btGjkEpa3.h:125
EPA::sHorizon::ff
sFace * ff
Definition: btGjkEpa3.h:567
GJK_SIMPLEX3_EPS
#define GJK_SIMPLEX3_EPS
Definition: btGjkEpa3.h:65
EPA::sFace::d
btScalar d
Definition: btGjkEpa3.h:551
GJK::removevertice
void removevertice(sSimplex &simplex)
Definition: btGjkEpa3.h:383
EPA::sHorizon::cf
sFace * cf
Definition: btGjkEpa3.h:566
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:46
btTransform.h
GJK_MAX_ITERATIONS
#define GJK_MAX_ITERATIONS
Definition: btGjkEpa3.h:60
GJK::m_nfree
U m_nfree
Definition: btGjkEpa3.h:156
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
GJK::projectorigin
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &d, btScalar *w, U &m)
Definition: btGjkEpa3.h:477
GJK::Initialize
void Initialize()
Definition: btGjkEpa3.h:167
btMatrix3x3::transposeTimes
btMatrix3x3 transposeTimes(const btMatrix3x3 &m) const
Definition: btMatrix3x3.h:1090
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
GJK::sSimplex::c
sSV * c[4]
Definition: btGjkEpa3.h:143
eEpaDegenerated
Definition: btGjkEpa3.h:532
EPA::sFace::n
btVector3 n
Definition: btGjkEpa3.h:550
btGjkEpaSolver3_Penetration
bool btGjkEpaSolver3_Penetration(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
Definition: btGjkEpa3.h:931
GJK::sSV
Definition: btGjkEpa3.h:137
btGjkEpaSolver3
Definition: btGjkEpa3.h:32
EPA::append
static void append(sList &list, sFace *face)
Definition: btGjkEpa3.h:595
GJK::sSV::w
btVector3 w
Definition: btGjkEpa3.h:139
EPA::m_result
GJK< btConvexTemplate >::sSimplex m_result
Definition: btGjkEpa3.h:574
btTransform::inverseTimes
btTransform inverseTimes(const btTransform &t) const
Return the inverse of this transform times the other transform.
Definition: btTransform.h:222
btSwap
void btSwap(T &a, T &b)
Definition: btScalar.h:643
EPA::expand
bool expand(U pass, typename GJK< btConvexTemplate >::sSV *w, sFace *f, U e, sHorizon &horizon)
Definition: btGjkEpa3.h:838
EPA::sList
Definition: btGjkEpa3.h:558
eEpaValid
Definition: btGjkEpa3.h:530
EPA::m_sv_store
GJK< btConvexTemplate >::sSV m_sv_store[EPA_MAX_VERTICES]
Definition: btGjkEpa3.h:577
btDot
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:890
eEpaOutOfVertices
Definition: btGjkEpa3.h:536
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
btGjkEpaSolver3::sResults::Penetrating
Definition: btGjkEpa3.h:39
btGjkEpaSolver3::sResults::Separated
Definition: btGjkEpa3.h:38
EPA::Evaluate
eEpaStatus Evaluate(GJK< btConvexTemplate > &gjk, const btVector3 &guess)
Definition: btGjkEpa3.h:622
eEpaFallBack
Definition: btGjkEpa3.h:538
MinkowskiDiff::m_toshape0
btTransform m_toshape0
Definition: btGjkEpa3.h:89
btGjkEpaSolver3_Distance
bool btGjkEpaSolver3_Distance(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
Definition: btGjkEpa3.h:898
btGjkEpaSolver3::sResults::EPA_Failed
Definition: btGjkEpa3.h:41
GJK::m_shape
MinkowskiDiff< btConvexTemplate > m_shape
Definition: btGjkEpa3.h:150
EPA_ACCURACY
#define EPA_ACCURACY
Definition: btGjkEpa3.h:72
EPA::sHorizon::nf
U nf
Definition: btGjkEpa3.h:568
GJK::EncloseOrigin
bool EncloseOrigin()
Definition: btGjkEpa3.h:312
eGjkInside
Definition: btGjkEpa3.h:128
GJK::appendvertice
void appendvertice(sSimplex &simplex, const btVector3 &v)
Definition: btGjkEpa3.h:387
U1
unsigned char U1
Definition: btGjkEpa3.h:79
GJK_MIN_DISTANCE
#define GJK_MIN_DISTANCE
Definition: btGjkEpa3.h:62
GJK::GJK
GJK(const btConvexTemplate &a, const btConvexTemplate &b)
Definition: btGjkEpa3.h:162
EPA::m_normal
btVector3 m_normal
Definition: btGjkEpa3.h:575
GJK::det
static btScalar det(const btVector3 &a, const btVector3 &b, const btVector3 &c)
Definition: btGjkEpa3.h:393
GJK::sSV::d
btVector3 d
Definition: btGjkEpa3.h:139
EPA::m_nextsv
U m_nextsv
Definition: btGjkEpa3.h:579
EPA::sFace::l
sFace * l[2]
Definition: btGjkEpa3.h:554
btGjkEpaSolver3::sResults::eStatus
eStatus
Definition: btGjkEpa3.h:36
EPA::EPA
EPA()
Definition: btGjkEpa3.h:583
btGjkEpaSolver3::sResults::witnesses
btVector3 witnesses[2]
Definition: btGjkEpa3.h:43
MinkowskiDiff::Support
btVector3 Support(const btVector3 &d, U index) const
Definition: btGjkEpa3.h:116
GJK::m_free
sSV * m_free[4]
Definition: btGjkEpa3.h:155
EPA::m_hull
sList m_hull
Definition: btGjkEpa3.h:580
EPA::m_depth
btScalar m_depth
Definition: btGjkEpa3.h:576
length
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:895
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
GJK_ACCURARY
#define GJK_ACCURARY
Definition: btGjkEpa3.h:61
EPA::sList::count
U count
Definition: btGjkEpa3.h:561
GJK::sSimplex::rank
U rank
Definition: btGjkEpa3.h:145
eEpaAccuraryReached
Definition: btGjkEpa3.h:537
eEpaInvalidHull
Definition: btGjkEpa3.h:534
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition: btSoftBodyHelpers.cpp:94
GJK::m_current
U m_current
Definition: btGjkEpa3.h:157
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
EPA::m_fc_store
sFace m_fc_store[EPA_MAX_FACES]
Definition: btGjkEpa3.h:578
MinkowskiDiff::m_convexAPtr
const btConvexTemplate * m_convexAPtr
Definition: btGjkEpa3.h:85
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
EPA::findbest
sFace * findbest()
Definition: btGjkEpa3.h:823
btGjkEpaSolver3::sResults::status
enum btGjkEpaSolver3::sResults::eStatus status