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