1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2014 Erwin Coumans https://bulletphysics.org
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"
30 #include "btGjkCollisionDescription.h"
31
32 struct btGjkEpaSolver3
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;
43 btVector3 witnesses[2];
44 btVector3 normal;
45 btScalar distance;
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>
83 struct MinkowskiDiff
84 {
85 const btConvexTemplate* m_convexAPtr;
86 const btConvexTemplate* m_convexBPtr;
87
88 btMatrix3x3 m_toshape1;
89 btTransform m_toshape0;
90
91 bool m_enableMargin;
92
MinkowskiDiffMinkowskiDiff93 MinkowskiDiff(const btConvexTemplate& a, const btConvexTemplate& b)
94 : m_convexAPtr(&a),
95 m_convexBPtr(&b)
96 {
97 }
98
EnableMarginMinkowskiDiff99 void EnableMargin(bool enable)
100 {
101 m_enableMargin = enable;
102 }
Support0MinkowskiDiff103 inline btVector3 Support0(const btVector3& d) const
104 {
105 return m_convexAPtr->getLocalSupportWithMargin(d);
106 }
Support1MinkowskiDiff107 inline btVector3 Support1(const btVector3& d) const
108 {
109 return m_toshape0 * m_convexBPtr->getLocalSupportWithMargin(m_toshape1 * d);
110 }
111
SupportMinkowskiDiff112 inline btVector3 Support(const btVector3& d) const
113 {
114 return (Support0(d) - Support1(-d));
115 }
SupportMinkowskiDiff116 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
125 enum eGjkStatus
126 {
127 eGjkValid,
128 eGjkInside,
129 eGjkFailed
130 };
131
132 // GJK
133 template <typename btConvexTemplate>
134 struct GJK
135 {
136 /* Types */
137 struct sSV
138 {
139 btVector3 d, w;
140 };
141 struct sSimplex
142 {
143 sSV* c[4];
144 btScalar p[4];
145 U rank;
146 };
147
148 /* Fields */
149
150 MinkowskiDiff<btConvexTemplate> m_shape;
151 btVector3 m_ray;
152 btScalar m_distance;
153 sSimplex m_simplices[2];
154 sSV m_store[4];
155 sSV* m_free[4];
156 U m_nfree;
157 U m_current;
158 sSimplex* m_simplex;
159 eGjkStatus m_status;
160 /* Methods */
161
GJKGJK162 GJK(const btConvexTemplate& a, const btConvexTemplate& b)
163 : m_shape(a, b)
164 {
165 Initialize();
166 }
InitializeGJK167 void Initialize()
168 {
169 m_ray = btVector3(0, 0, 0);
170 m_nfree = 0;
171 m_status = eGjkFailed;
172 m_current = 0;
173 m_distance = 0;
174 }
EvaluateGJK175 eGjkStatus Evaluate(const MinkowskiDiff<btConvexTemplate>& shapearg, const btVector3& guess)
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;
189 m_status = eGjkValid;
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;
208 sSimplex& cs = m_simplices[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 */
214 m_status = eGjkInside;
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 */
231 removevertice(m_simplices[m_current]);
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 */
243 removevertice(m_simplices[m_current]);
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 */
292 removevertice(m_simplices[m_current]);
293 break;
294 }
295 m_status = ((++iterations) < GJK_MAX_ITERATIONS) ? m_status : eGjkFailed;
296 } while (m_status == eGjkValid);
297 m_simplex = &m_simplices[m_current];
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 }
EncloseOriginGJK312 bool EncloseOrigin()
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);
324 removevertice(*m_simplex);
325 appendvertice(*m_simplex, -axis);
326 if (EncloseOrigin()) return (true);
327 removevertice(*m_simplex);
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 {
341 appendvertice(*m_simplex, p);
342 if (EncloseOrigin()) return (true);
343 removevertice(*m_simplex);
344 appendvertice(*m_simplex, -p);
345 if (EncloseOrigin()) return (true);
346 removevertice(*m_simplex);
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 {
357 appendvertice(*m_simplex, n);
358 if (EncloseOrigin()) return (true);
359 removevertice(*m_simplex);
360 appendvertice(*m_simplex, -n);
361 if (EncloseOrigin()) return (true);
362 removevertice(*m_simplex);
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 */
getsupportGJK378 void getsupport(const btVector3& d, sSV& sv) const
379 {
380 sv.d = d / d.length();
381 sv.w = m_shape.Support(sv.d);
382 }
removeverticeGJK383 void removevertice(sSimplex& simplex)
384 {
385 m_free[m_nfree++] = simplex.c[--simplex.rank];
386 }
appendverticeGJK387 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 }
detGJK393 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 }
projectoriginGJK399 static btScalar projectorigin(const btVector3& a,
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 }
projectoriginGJK431 static btScalar projectorigin(const btVector3& a,
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 }
projectoriginGJK477 static btScalar projectorigin(const btVector3& a,
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
528 enum eEpaStatus
529 {
530 eEpaValid,
531 eEpaTouching,
532 eEpaDegenerated,
533 eEpaNonConvex,
534 eEpaInvalidHull,
535 eEpaOutOfFaces,
536 eEpaOutOfVertices,
537 eEpaAccuraryReached,
538 eEpaFallBack,
539 eEpaFailed
540 };
541
542 // EPA
543 template <typename btConvexTemplate>
544 struct EPA
545 {
546 /* Types */
547
548 struct sFace
549 {
550 btVector3 n;
551 btScalar d;
552 typename GJK<btConvexTemplate>::sSV* c[3];
553 sFace* f[3];
554 sFace* l[2];
555 U1 e[3];
556 U1 pass;
557 };
558 struct sList
559 {
560 sFace* root;
561 U count;
sListEPA::sList562 sList() : root(0), count(0) {}
563 };
564 struct sHorizon
565 {
566 sFace* cf;
567 sFace* ff;
568 U nf;
sHorizonEPA::sHorizon569 sHorizon() : cf(0), ff(0), nf(0) {}
570 };
571
572 /* Fields */
573 eEpaStatus m_status;
574 typename GJK<btConvexTemplate>::sSimplex m_result;
575 btVector3 m_normal;
576 btScalar m_depth;
577 typename GJK<btConvexTemplate>::sSV m_sv_store[EPA_MAX_VERTICES];
578 sFace m_fc_store[EPA_MAX_FACES];
579 U m_nextsv;
580 sList m_hull;
581 sList m_stock;
582 /* Methods */
EPAEPA583 EPA()
584 {
585 Initialize();
586 }
587
bindEPA588 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 }
appendEPA595 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 }
removeEPA603 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
InitializeEPA611 void Initialize()
612 {
613 m_status = eEpaFailed;
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 {
619 append(m_stock, &m_fc_store[EPA_MAX_FACES - i - 1]);
620 }
621 }
EvaluateEPA622 eEpaStatus Evaluate(GJK<btConvexTemplate>& gjk, const btVector3& guess)
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 }
634 m_status = eEpaValid;
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);
661 m_status = eEpaValid;
662 for (; iterations < EPA_MAX_ITERATIONS; ++iterations)
663 {
664 if (m_nextsv < EPA_MAX_VERTICES)
665 {
666 sHorizon horizon;
667 typename GJK<btConvexTemplate>::sSV* w = &m_sv_store[m_nextsv++];
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 {
690 m_status = eEpaInvalidHull;
691 break;
692 }
693 }
694 else
695 {
696 m_status = eEpaAccuraryReached;
697 break;
698 }
699 }
700 else
701 {
702 m_status = eEpaOutOfVertices;
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 */
730 m_status = eEpaFallBack;
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 }
getedgedistEPA743 bool getedgedist(sFace* face, typename GJK<btConvexTemplate>::sSV* a, typename GJK<btConvexTemplate>::sSV* b, btScalar& dist)
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 }
newfaceEPA779 sFace* newface(typename GJK<btConvexTemplate>::sSV* a, typename GJK<btConvexTemplate>::sSV* b, typename GJK<btConvexTemplate>::sSV* c, bool forced)
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
811 m_status = eEpaNonConvex;
812 }
813 else
814 m_status = eEpaDegenerated;
815
816 remove(m_hull, face);
817 append(m_stock, face);
818 return 0;
819 }
820 m_status = m_stock.root ? eEpaOutOfVertices : eEpaOutOfFaces;
821 return 0;
822 }
findbestEPA823 sFace* findbest()
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 }
expandEPA838 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>
Initialize(const btConvexTemplate & a,const btConvexTemplate & b,btGjkEpaSolver3::sResults & results,MinkowskiDiff<btConvexTemplate> & shape)878 static void Initialize(const btConvexTemplate& a, const btConvexTemplate& b,
879 btGjkEpaSolver3::sResults& results,
880 MinkowskiDiff<btConvexTemplate>& shape)
881 {
882 /* Results */
883 results.witnesses[0] =
884 results.witnesses[1] = btVector3(0, 0, 0);
885 results.status = btGjkEpaSolver3::sResults::Separated;
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>
btGjkEpaSolver3_Distance(const btConvexTemplate & a,const btConvexTemplate & b,const btVector3 & guess,btGjkEpaSolver3::sResults & results)898 bool btGjkEpaSolver3_Distance(const btConvexTemplate& a, const btConvexTemplate& b,
899 const btVector3& guess,
900 btGjkEpaSolver3::sResults& results)
901 {
902 MinkowskiDiff<btConvexTemplate> shape(a, b);
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 {
925 results.status = gjk_status == eGjkInside ? btGjkEpaSolver3::sResults::Penetrating : btGjkEpaSolver3::sResults::GJK_Failed;
926 return (false);
927 }
928 }
929
930 template <typename btConvexTemplate>
btGjkEpaSolver3_Penetration(const btConvexTemplate & a,const btConvexTemplate & b,const btVector3 & guess,btGjkEpaSolver3::sResults & results)931 bool btGjkEpaSolver3_Penetration(const btConvexTemplate& a,
932 const btConvexTemplate& b,
933 const btVector3& guess,
934 btGjkEpaSolver3::sResults& results)
935 {
936 MinkowskiDiff<btConvexTemplate> shape(a, b);
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 {
944 EPA<btConvexTemplate> epa;
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 }
953 results.status = btGjkEpaSolver3::sResults::Penetrating;
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
961 results.status = btGjkEpaSolver3::sResults::EPA_Failed;
962 }
963 break;
964 case eGjkFailed:
965 results.status = btGjkEpaSolver3::sResults::GJK_Failed;
966 break;
967 default:
968 {
969 }
970 }
971 return (false);
972 }
973
974 #if 0
975 int btComputeGjkEpaPenetration2(const btCollisionDescription& colDesc, btDistanceInfo* distInfo)
976 {
977 btGjkEpaSolver3::sResults results;
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>
btComputeGjkDistance(const btConvexTemplate & a,const btConvexTemplate & b,const btGjkCollisionDescription & colDesc,btDistanceInfoTemplate * distInfo)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
1064