Bullet Collision Detection & Physics Library
btGjkEpa3.h
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2014 Erwin Coumans https://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the
7use of this software.
8Permission is granted to anyone to use this software for any purpose,
9including commercial applications, and to alter it and redistribute it
10freely,
11subject to the following restrictions:
12
131. The origin of this software must not be misrepresented; you must not
14claim that you wrote the original software. If you use this software in a
15product, an acknowledgment in the product documentation would be appreciated
16but is not required.
172. Altered source versions must be plainly marked as such, and must not be
18misrepresented as being the original software.
193. This notice may not be removed or altered from any source distribution.
20*/
21
22/*
23Initial GJK-EPA collision solver by Nathanael Presson, 2008
24Improvements and refactoring by Erwin Coumans, 2008-2014
25*/
26#ifndef BT_GJK_EPA3_H
27#define BT_GJK_EPA3_H
28
31
33{
34 struct sResults
35 {
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 */
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
78typedef unsigned int U;
79typedef unsigned char U1;
80
81// MinkowskiDiff
82template <typename btConvexTemplate>
84{
87
90
92
94 : m_convexAPtr(&a),
95 m_convexBPtr(&b)
96 {
97 }
98
100 {
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{
131
132// GJK
133template <typename btConvexTemplate>
134struct GJK
135{
136 /* Types */
137 struct sSV
138 {
140 };
141 struct sSimplex
142 {
143 sSV* c[4];
146 };
147
148 /* Fields */
149
160 /* Methods */
161
163 : m_shape(a, b)
164 {
165 Initialize();
166 }
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;
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 */
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;
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 }
296 } while (m_status == eGjkValid);
298 switch (m_status)
299 {
300 case eGjkValid:
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);
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);
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 }
384 {
385 m_free[m_nfree++] = simplex.c[--simplex.rank];
386 }
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{
541
542// EPA
543template <typename btConvexTemplate>
544struct 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 {
569 sHorizon() : cf(0), ff(0), nf(0) {}
570 };
571
572 /* Fields */
582 /* Methods */
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
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);
663 {
665 {
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);
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 {
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
877template <typename btConvexTemplate>
878static void Initialize(const btConvexTemplate& a, const btConvexTemplate& b,
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//
897template <typename btConvexTemplate>
899 const btVector3& guess,
901{
903 Initialize(a, b, results, shape);
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
930template <typename btConvexTemplate>
932 const btConvexTemplate& b,
933 const btVector3& guess,
935{
937 Initialize(a, b, results, shape);
939 eGjkStatus gjk_status = gjk.Evaluate(shape, -guess);
940 switch (gjk_status)
941 {
942 case eGjkInside:
943 {
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
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];
996 {
997 tmpNormalInB = results.normal;
998 lenSqr = results.normal.length2();
999 }
1000
1002 {
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
1023template <typename btConvexTemplate, typename btDistanceInfoTemplate>
1026{
1028 btVector3 guess = colDesc.m_firstDir;
1029
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
#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
const T & btMax(const T &a, const T &b)
Definition btMinMax.h:27
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
btScalar btSqrt(btScalar y)
Definition btScalar.h:466
btScalar btFabs(btScalar x)
Definition btScalar.h:497
#define SIMD_EPSILON
Definition btScalar.h:543
void btSwap(T &a, T &b)
Definition btScalar.h:643
static T sum(const btAlignedObjectArray< T > &items)
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition btVector3.h:890
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition btVector3.h:918
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition btMatrix3x3.h:50
btMatrix3x3 transposeTimes(const btMatrix3x3 &m) const
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition btTransform.h:30
btTransform inverseTimes(const btTransform &t) const
Return the inverse of this transform times the other transform.
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:82
const btScalar & w() const
Return the w value.
Definition btVector3.h:581
const btScalar & z() const
Return the z value.
Definition btVector3.h:579
btScalar length() const
Return the length of the vector.
Definition btVector3.h:257
btScalar distance(const btVector3 &v) const
Return the distance between the ends of this and another vector This is symantically treating the vec...
Definition btVector3.h:944
btScalar length2() const
Return the length of the vector squared.
Definition btVector3.h:251
const btScalar & x() const
Return the x value.
Definition btVector3.h:575
const btScalar & y() const
Return the y value.
Definition btVector3.h:577
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
btVector3 n
Definition btGjkEpa3.h:550
sFace * root
Definition btGjkEpa3.h:560
bool getedgedist(sFace *face, typename GJK< btConvexTemplate >::sSV *a, typename GJK< btConvexTemplate >::sSV *b, btScalar &dist)
Definition btGjkEpa3.h:743
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
sFace * newface(typename GJK< btConvexTemplate >::sSV *a, typename GJK< btConvexTemplate >::sSV *b, typename GJK< btConvexTemplate >::sSV *c, bool forced)
Definition btGjkEpa3.h:779
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
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
sFace * findbest()
Definition btGjkEpa3.h:823
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
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