Blender  V3.3
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 {
129  eGjkFailed
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 {
539  eEpaFailed
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
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble y2 _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat y2 _GL_VOID_RET _GL_VOID GLint GLint GLint y2 _GL_VOID_RET _GL_VOID GLshort GLshort GLshort y2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLuint *buffer _GL_VOID_RET _GL_VOID GLdouble t _GL_VOID_RET _GL_VOID GLfloat t _GL_VOID_RET _GL_VOID GLint t _GL_VOID_RET _GL_VOID GLshort t _GL_VOID_RET _GL_VOID GLdouble t
ATTR_WARN_UNUSED_RESULT const BMLoop * l
ATTR_WARN_UNUSED_RESULT const BMVert const BMEdge * e
ATTR_WARN_UNUSED_RESULT const BMVert * v
#define EPA_ACCURACY
Definition: btGjkEpa3.h:72
int btComputeGjkDistance(const btConvexTemplate &a, const btConvexTemplate &b, const btGjkCollisionDescription &colDesc, btDistanceInfoTemplate *distInfo)
Definition: btGjkEpa3.h:1024
unsigned int U
Definition: btGjkEpa3.h:78
#define GJK_ACCURARY
Definition: btGjkEpa3.h:61
#define EPA_MAX_FACES
Definition: btGjkEpa3.h:70
static void Initialize(const btConvexTemplate &a, const btConvexTemplate &b, btGjkEpaSolver3::sResults &results, MinkowskiDiff< btConvexTemplate > &shape)
Definition: btGjkEpa3.h:878
#define GJK_MIN_DISTANCE
Definition: btGjkEpa3.h:62
#define GJK_SIMPLEX4_EPS
Definition: btGjkEpa3.h:66
#define EPA_PLANE_EPS
Definition: btGjkEpa3.h:74
#define GJK_DUPLICATED_EPS
Definition: btGjkEpa3.h:63
bool btGjkEpaSolver3_Penetration(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
Definition: btGjkEpa3.h:931
eEpaStatus
Definition: btGjkEpa3.h:529
@ eEpaFailed
Definition: btGjkEpa3.h:539
@ eEpaFallBack
Definition: btGjkEpa3.h:538
@ eEpaDegenerated
Definition: btGjkEpa3.h:532
@ eEpaInvalidHull
Definition: btGjkEpa3.h:534
@ eEpaNonConvex
Definition: btGjkEpa3.h:533
@ eEpaOutOfVertices
Definition: btGjkEpa3.h:536
@ eEpaTouching
Definition: btGjkEpa3.h:531
@ eEpaAccuraryReached
Definition: btGjkEpa3.h:537
@ eEpaValid
Definition: btGjkEpa3.h:530
@ eEpaOutOfFaces
Definition: btGjkEpa3.h:535
#define EPA_MAX_VERTICES
Definition: btGjkEpa3.h:69
#define GJK_SIMPLEX2_EPS
Definition: btGjkEpa3.h:64
#define GJK_MAX_ITERATIONS
Definition: btGjkEpa3.h:60
#define GJK_SIMPLEX3_EPS
Definition: btGjkEpa3.h:65
bool btGjkEpaSolver3_Distance(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
Definition: btGjkEpa3.h:898
unsigned char U1
Definition: btGjkEpa3.h:79
eGjkStatus
Definition: btGjkEpa3.h:126
@ eGjkValid
Definition: btGjkEpa3.h:127
@ eGjkInside
Definition: btGjkEpa3.h:128
@ eGjkFailed
Definition: btGjkEpa3.h:129
#define EPA_MAX_ITERATIONS
Definition: btGjkEpa3.h:71
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
SIMD_FORCE_INLINE const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
SIMD_FORCE_INLINE const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:119
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
SIMD_FORCE_INLINE btScalar btFabs(btScalar x)
Definition: btScalar.h:497
SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
SIMD_FORCE_INLINE void btSwap(T &a, T &b)
Definition: btScalar.h:643
#define SIMD_EPSILON
Definition: btScalar.h:543
static T sum(const btAlignedObjectArray< T > &items)
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
SIMD_FORCE_INLINE btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
SIMD_FORCE_INLINE btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:890
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
SIMD_FORCE_INLINE btScalar distance2(const btVector3 &v) const
Return the distance squared between the ends of this and another vector This is symantically treating...
Definition: btVector3.h:939
SIMD_FORCE_INLINE btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:918
BLI_INLINE float fb(float length, float L)
ccl_device_inline float4 mask(const int4 &mask, const float4 &a)
Definition: math_float4.h:513
static ulong * next
static unsigned c
Definition: RandGen.cpp:83
static unsigned a[3]
Definition: RandGen.cpp:78
T length(const vec_base< T, Size > &a)
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
sFace * l[2]
Definition: btGjkEpa3.h:554
sFace * f[3]
Definition: btGjkEpa3.h:553
GJK< btConvexTemplate >::sSV * c[3]
Definition: btGjkEpa3.h:552
btScalar d
Definition: btGjkEpa3.h:551
U1 e[3]
Definition: btGjkEpa3.h:555
btVector3 n
Definition: btGjkEpa3.h:550
sFace * cf
Definition: btGjkEpa3.h:566
sFace * ff
Definition: btGjkEpa3.h:567
sFace * root
Definition: btGjkEpa3.h:560
Definition: btGjkEpa3.h:545
bool getedgedist(sFace *face, typename GJK< btConvexTemplate >::sSV *a, typename GJK< btConvexTemplate >::sSV *b, btScalar &dist)
Definition: btGjkEpa3.h:743
sFace * newface(typename GJK< btConvexTemplate >::sSV *a, typename GJK< btConvexTemplate >::sSV *b, typename GJK< btConvexTemplate >::sSV *c, bool forced)
Definition: btGjkEpa3.h:779
bool expand(U pass, typename GJK< btConvexTemplate >::sSV *w, sFace *f, U e, sHorizon &horizon)
Definition: btGjkEpa3.h:838
static void append(sList &list, sFace *face)
Definition: btGjkEpa3.h:595
GJK< btConvexTemplate >::sSimplex m_result
Definition: btGjkEpa3.h:574
GJK< btConvexTemplate >::sSV m_sv_store[EPA_MAX_VERTICES]
Definition: btGjkEpa3.h:577
sList m_stock
Definition: btGjkEpa3.h:581
static void remove(sList &list, sFace *face)
Definition: btGjkEpa3.h:603
eEpaStatus Evaluate(GJK< btConvexTemplate > &gjk, const btVector3 &guess)
Definition: btGjkEpa3.h:622
btScalar m_depth
Definition: btGjkEpa3.h:576
U m_nextsv
Definition: btGjkEpa3.h:579
sList m_hull
Definition: btGjkEpa3.h:580
static void bind(sFace *fa, U ea, sFace *fb, U eb)
Definition: btGjkEpa3.h:588
sFace * findbest()
Definition: btGjkEpa3.h:823
btVector3 m_normal
Definition: btGjkEpa3.h:575
sFace m_fc_store[EPA_MAX_FACES]
Definition: btGjkEpa3.h:578
void Initialize()
Definition: btGjkEpa3.h:611
eEpaStatus m_status
Definition: btGjkEpa3.h:573
EPA()
Definition: btGjkEpa3.h:583
btVector3 w
Definition: btGjkEpa3.h:139
btVector3 d
Definition: btGjkEpa3.h:139
sSV * c[4]
Definition: btGjkEpa3.h:143
btScalar p[4]
Definition: btGjkEpa3.h:144
Definition: btGjkEpa3.h:135
U m_nfree
Definition: btGjkEpa3.h:156
eGjkStatus Evaluate(const MinkowskiDiff< btConvexTemplate > &shapearg, const btVector3 &guess)
Definition: btGjkEpa3.h:175
void getsupport(const btVector3 &d, sSV &sv) const
Definition: btGjkEpa3.h:378
MinkowskiDiff< btConvexTemplate > m_shape
Definition: btGjkEpa3.h:150
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &d, btScalar *w, U &m)
Definition: btGjkEpa3.h:477
U m_current
Definition: btGjkEpa3.h:157
bool EncloseOrigin()
Definition: btGjkEpa3.h:312
sSV * m_free[4]
Definition: btGjkEpa3.h:155
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, btScalar *w, U &m)
Definition: btGjkEpa3.h:399
sSV m_store[4]
Definition: btGjkEpa3.h:154
sSimplex * m_simplex
Definition: btGjkEpa3.h:158
static btScalar det(const btVector3 &a, const btVector3 &b, const btVector3 &c)
Definition: btGjkEpa3.h:393
void Initialize()
Definition: btGjkEpa3.h:167
GJK(const btConvexTemplate &a, const btConvexTemplate &b)
Definition: btGjkEpa3.h:162
sSimplex m_simplices[2]
Definition: btGjkEpa3.h:153
btVector3 m_ray
Definition: btGjkEpa3.h:151
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, btScalar *w, U &m)
Definition: btGjkEpa3.h:431
void removevertice(sSimplex &simplex)
Definition: btGjkEpa3.h:383
eGjkStatus m_status
Definition: btGjkEpa3.h:159
btScalar m_distance
Definition: btGjkEpa3.h:152
void appendvertice(sSimplex &simplex, const btVector3 &v)
Definition: btGjkEpa3.h:387
btVector3 Support1(const btVector3 &d) const
Definition: btGjkEpa3.h:107
btVector3 Support0(const btVector3 &d) const
Definition: btGjkEpa3.h:103
void EnableMargin(bool enable)
Definition: btGjkEpa3.h:99
btVector3 Support(const btVector3 &d) const
Definition: btGjkEpa3.h:112
btVector3 Support(const btVector3 &d, U index) const
Definition: btGjkEpa3.h:116
MinkowskiDiff(const btConvexTemplate &a, const btConvexTemplate &b)
Definition: btGjkEpa3.h:93
btTransform m_toshape0
Definition: btGjkEpa3.h:89
const btConvexTemplate * m_convexBPtr
Definition: btGjkEpa3.h:86
const btConvexTemplate * m_convexAPtr
Definition: btGjkEpa3.h:85
bool m_enableMargin
Definition: btGjkEpa3.h:91
btMatrix3x3 m_toshape1
Definition: btGjkEpa3.h:88
enum btGjkEpaSolver3::sResults::eStatus status
btVector3 witnesses[2]
Definition: btGjkEpa3.h:43