Blender  V3.3
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  {
103  return m_shapes[0]->localGetSupportVertexNonVirtual(d);
104  }
105  else
106  {
107  return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d);
108  }
109  }
110  inline btVector3 Support1(const btVector3& d) const
111  {
112  if (m_enableMargin)
113  {
114  return m_toshape0 * (m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1 * d));
115  }
116  else
117  {
118  return m_toshape0 * (m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1 * d));
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  {
174  Failed
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  {
595  Failed
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  results.distance = length - margin;
1053  return results.distance;
1054  }
1055  else
1056  {
1057  if (gjk_status == GJK::eStatus::Inside)
1058  {
1059  if (Penetration(shape0, wtrs0, &shape1, wtrs1, gjk.m_ray, results))
1060  {
1061  const btVector3 delta = results.witnesses[0] -
1062  results.witnesses[1];
1063  const btScalar length = delta.length();
1064  if (length >= SIMD_EPSILON)
1065  results.normal = delta / length;
1066  return (-length);
1067  }
1068  }
1069  }
1070  return (SIMD_INFINITY);
1071 }
1072 
1073 //
1075  const btTransform& wtrs0,
1076  const btConvexShape* shape1,
1077  const btTransform& wtrs1,
1078  const btVector3& guess,
1079  sResults& results)
1080 {
1081  if (!Distance(shape0, wtrs0, shape1, wtrs1, guess, results))
1082  return (Penetration(shape0, wtrs0, shape1, wtrs1, guess, results, false));
1083  else
1084  return (true);
1085 }
1086 #endif //__SPU__
1087 
1088 /* Symbols cleanup */
1089 
1090 #undef GJK_MAX_ITERATIONS
1091 #undef GJK_ACCURACY
1092 #undef GJK_MIN_DISTANCE
1093 #undef GJK_DUPLICATED_EPS
1094 #undef GJK_SIMPLEX2_EPS
1095 #undef GJK_SIMPLEX3_EPS
1096 #undef GJK_SIMPLEX4_EPS
1097 
1098 #undef EPA_MAX_VERTICES
1099 #undef EPA_MAX_FACES
1100 #undef EPA_MAX_ITERATIONS
1101 #undef EPA_ACCURACY
1102 #undef EPA_FALLBACK
1103 #undef EPA_PLANE_EPS
1104 #undef EPA_INSIDE_EPS
_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
btConvexShape()
not supported on IBM SDK, until we fix the alignment of btVector3
btVector3 localGetSupportVertexNonVirtual(const btVector3 &vec) const
btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3 &vec) const
#define EPA_ACCURACY
Definition: btGjkEpa2.cpp:67
#define EPA_MAX_FACES
Definition: btGjkEpa2.cpp:73
#define GJK_MIN_DISTANCE
Definition: btGjkEpa2.cpp:50
#define GJK_SIMPLEX4_EPS
Definition: btGjkEpa2.cpp:56
#define EPA_PLANE_EPS
Definition: btGjkEpa2.cpp:68
#define GJK_DUPLICATED_EPS
Definition: btGjkEpa2.cpp:51
#define GJK_ACCURACY
Definition: btGjkEpa2.cpp:49
#define EPA_MAX_VERTICES
Definition: btGjkEpa2.cpp:59
#define GJK_SIMPLEX2_EPS
Definition: btGjkEpa2.cpp:54
#define GJK_MAX_ITERATIONS
Definition: btGjkEpa2.cpp:42
#define GJK_SIMPLEX3_EPS
Definition: btGjkEpa2.cpp:55
#define EPA_MAX_ITERATIONS
Definition: btGjkEpa2.cpp:60
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
#define SIMD_INFINITY
Definition: btScalar.h:544
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)
btSphereShape(btScalar radius)
Definition: btSphereShape.h:29
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 btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:918
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:50
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 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
MinkowskiDiff tShape
Definition: btGjkEpa2.cpp:152
unsigned int U
Definition: btGjkEpa2.cpp:76
unsigned char U1
Definition: btGjkEpa2.cpp:77
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
enum btGjkEpaSolver2::sResults::eStatus status
btVector3 witnesses[2]
Definition: btGjkEpa2.h:42
static btScalar SignedDistance(const btVector3 &position, btScalar margin, const btConvexShape *shape, const btTransform &wtrs, sResults &results)
Definition: btGjkEpa2.cpp:1021
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
static bool Distance(const btConvexShape *shape0, const btTransform &wtrs0, const btConvexShape *shape1, const btTransform &wtrs1, const btVector3 &guess, sResults &results)
Definition: btGjkEpa2.cpp:937
static int StackSizeRequirement()
Definition: btGjkEpa2.cpp:931
bool expand(U pass, sSV *w, sFace *f, U e, sHorizon &horizon)
Definition: btGjkEpa2.cpp:864
static void remove(sList &list, sFace *face)
Definition: btGjkEpa2.cpp:629
btVector3 m_normal
Definition: btGjkEpa2.cpp:601
GJK::sSimplex m_result
Definition: btGjkEpa2.cpp:600
sSV m_sv_store[EPA_MAX_VERTICES]
Definition: btGjkEpa2.cpp:603
eStatus::_ Evaluate(GJK &gjk, const btVector3 &guess)
Definition: btGjkEpa2.cpp:648
sFace * findbest()
Definition: btGjkEpa2.cpp:849
sFace m_fc_store[EPA_MAX_FACES]
Definition: btGjkEpa2.cpp:604
static void append(sList &list, sFace *face)
Definition: btGjkEpa2.cpp:621
sFace * newface(sSV *a, sSV *b, sSV *c, bool forced)
Definition: btGjkEpa2.cpp:805
eStatus::_ m_status
Definition: btGjkEpa2.cpp:599
static void bind(sFace *fa, U ea, sFace *fb, U eb)
Definition: btGjkEpa2.cpp:614
bool getedgedist(sFace *face, sSV *a, sSV *b, btScalar &dist)
Definition: btGjkEpa2.cpp:769
sSimplex m_simplices[2]
Definition: btGjkEpa2.cpp:181
sSimplex * m_simplex
Definition: btGjkEpa2.cpp:186
void removevertice(sSimplex &simplex)
Definition: btGjkEpa2.cpp:409
void getsupport(const btVector3 &d, sSV &sv) const
Definition: btGjkEpa2.cpp:404
btScalar m_distance
Definition: btGjkEpa2.cpp:180
eStatus::_ m_status
Definition: btGjkEpa2.cpp:187
static btScalar det(const btVector3 &a, const btVector3 &b, const btVector3 &c)
Definition: btGjkEpa2.cpp:419
void appendvertice(sSimplex &simplex, const btVector3 &v)
Definition: btGjkEpa2.cpp:413
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, btScalar *w, U &m)
Definition: btGjkEpa2.cpp:457
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &d, btScalar *w, U &m)
Definition: btGjkEpa2.cpp:503
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, btScalar *w, U &m)
Definition: btGjkEpa2.cpp:425
eStatus::_ Evaluate(const tShape &shapearg, const btVector3 &guess)
Definition: btGjkEpa2.cpp:201
btVector3 Support0(const btVector3 &d) const
Definition: btGjkEpa2.cpp:129
void EnableMargin(bool enable)
Definition: btGjkEpa2.cpp:122
const btConvexShape * m_shapes[2]
Definition: btGjkEpa2.cpp:82
btVector3 Support1(const btVector3 &d) const
Definition: btGjkEpa2.cpp:133
btVector3 Support(const btVector3 &d, U index) const
Definition: btGjkEpa2.cpp:143
btVector3(btConvexShape::* Ls)(const btVector3 &) const
Definition: btGjkEpa2.cpp:88
btVector3 Support(const btVector3 &d) const
Definition: btGjkEpa2.cpp:139