Bullet Collision Detection & Physics Library: btGjkEpa2.cpp Source File

Bullet Collision Detection & Physics Library

btGjkEpa2.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the
7 use of this software.
8 Permission is granted to anyone to use this software for any purpose,
9 including commercial applications, and to alter it and redistribute it
10 freely,
11 subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software in a
15 product, an acknowledgment in the product documentation would be appreciated
16 but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20 */
21
22 /*
23 GJK-EPA collision solver by Nathanael Presson, 2008
24 */
25 #include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
26 #include "BulletCollision/CollisionShapes/btSphereShape.h"
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
80 struct MinkowskiDiff
81 {
82 const btConvexShape* m_shapes[2];
83 btMatrix3x3 m_toshape1;
84 btTransform m_toshape0;
85 #ifdef __SPU__
86 bool m_enableMargin;
87 #else
88 btVector3 (btConvexShape::*Ls)(const btVector3&) const;
89 #endif //__SPU__
90
91 MinkowskiDiff()
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)
125 Ls = &btConvexShape::localGetSupportVertexNonVirtual;
126 else
127 Ls = &btConvexShape::localGetSupportVertexWithoutMarginNonVirtual;
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
152 typedef MinkowskiDiff tShape;
153
154 // GJK
155 struct GJK
156 {
157 /* Types */
158 struct sSV
159 {
160 btVector3 d, w;
161 };
162 struct sSimplex
163 {
164 sSV* c[4];
165 btScalar p[4];
166 U rank;
167 };
168 struct eStatus
169 {
170 enum _
171 {
172 Valid,
173 Inside,
174 Failed
175 };
176 };
177 /* Fields */
178 tShape m_shape;
179 btVector3 m_ray;
180 btScalar m_distance;
181 sSimplex m_simplices[2];
182 sSV m_store[4];
183 sSV* m_free[4];
184 U m_nfree;
185 U m_current;
186 sSimplex* m_simplex;
187 eStatus::_ m_status;
188 /* Methods */
189 GJK()
190 {
191 Initialize();
192 }
193 void Initialize()
194 {
195 m_ray = btVector3(0, 0, 0);
196 m_nfree = 0;
197 m_status = eStatus::Failed;
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;
215 m_status = eStatus::Valid;
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;
234 sSimplex& cs = m_simplices[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 */
240 m_status = eStatus::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 */
257 removevertice(m_simplices[m_current]);
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 */
269 removevertice(m_simplices[m_current]);
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 */
318 removevertice(m_simplices[m_current]);
319 break;
320 }
321 m_status = ((++iterations) < GJK_MAX_ITERATIONS) ? m_status : eStatus::Failed;
322 } while (m_status == eStatus::Valid);
323 m_simplex = &m_simplices[m_current];
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 }
338 bool EncloseOrigin()
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);
350 removevertice(*m_simplex);
351 appendvertice(*m_simplex, -axis);
352 if (EncloseOrigin()) return (true);
353 removevertice(*m_simplex);
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 {
367 appendvertice(*m_simplex, p);
368 if (EncloseOrigin()) return (true);
369 removevertice(*m_simplex);
370 appendvertice(*m_simplex, -p);
371 if (EncloseOrigin()) return (true);
372 removevertice(*m_simplex);
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 {
383 appendvertice(*m_simplex, n);
384 if (EncloseOrigin()) return (true);
385 removevertice(*m_simplex);
386 appendvertice(*m_simplex, -n);
387 if (EncloseOrigin()) return (true);
388 removevertice(*m_simplex);
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 }
425 static btScalar projectorigin(const btVector3& a,
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 }
457 static btScalar projectorigin(const btVector3& a,
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 }
503 static btScalar projectorigin(const btVector3& a,
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 {
561 btVector3 n;
562 btScalar d;
563 sSV* c[3];
564 sFace* f[3];
565 sFace* l[2];
566 U1 e[3];
567 U1 pass;
568 };
569 struct sList
570 {
571 sFace* root;
572 U count;
573 sList() : root(0), count(0) {}
574 };
575 struct sHorizon
576 {
577 sFace* cf;
578 sFace* ff;
579 U nf;
580 sHorizon() : cf(0), ff(0), nf(0) {}
581 };
582 struct eStatus
583 {
584 enum _
585 {
586 Valid,
587 Touching,
588 Degenerated,
589 NonConvex,
590 InvalidHull,
591 OutOfFaces,
592 OutOfVertices,
593 AccuraryReached,
594 FallBack,
595 Failed
596 };
597 };
598 /* Fields */
599 eStatus::_ m_status;
600 GJK::sSimplex m_result;
601 btVector3 m_normal;
602 btScalar m_depth;
603 sSV m_sv_store[EPA_MAX_VERTICES];
604 sFace m_fc_store[EPA_MAX_FACES];
605 U m_nextsv;
606 sList m_hull;
607 sList m_stock;
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 {
639 m_status = eStatus::Failed;
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 {
645 append(m_stock, &m_fc_store[EPA_MAX_FACES - i - 1]);
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 }
660 m_status = eStatus::Valid;
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);
687 m_status = eStatus::Valid;
688 for (; iterations < EPA_MAX_ITERATIONS; ++iterations)
689 {
690 if (m_nextsv < EPA_MAX_VERTICES)
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 {
716 m_status = eStatus::InvalidHull;
717 break;
718 }
719 }
720 else
721 {
722 m_status = eStatus::AccuraryReached;
723 break;
724 }
725 }
726 else
727 {
728 m_status = eStatus::OutOfVertices;
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 */
756 m_status = eStatus::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
837 m_status = eStatus::NonConvex;
838 }
839 else
840 m_status = eStatus::Degenerated;
841
842 remove(m_hull, face);
843 append(m_stock, face);
844 return 0;
845 }
846 m_status = m_stock.root ? eStatus::OutOfVertices : eStatus::OutOfFaces;
847 return 0;
848 }
849 sFace* findbest()
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);
913 results.status = btGjkEpaSolver2::sResults::Separated;
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 //
931 int btGjkEpaSolver2::StackSizeRequirement()
932 {
933 return (sizeof(GJK) + sizeof(EPA));
934 }
935
936 //
937 bool btGjkEpaSolver2::Distance(const btConvexShape* shape0,
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 //
973 bool btGjkEpaSolver2::Penetration(const btConvexShape* shape0,
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 {
987 case GJK::eStatus::Inside:
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 //
1021 btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position,
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 //
1074 bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0,
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
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
GJK_SIMPLEX2_EPS
#define GJK_SIMPLEX2_EPS
Definition: btGjkEpa2.cpp:54
btConvexInternalShape.h
gjkepa2_impl::GJK::getsupport
void getsupport(const btVector3 &d, sSV &sv) const
Definition: btGjkEpa2.cpp:404
gjkepa2_impl::GJK::Evaluate
eStatus::_ Evaluate(const tShape &shapearg, const btVector3 &guess)
Definition: btGjkEpa2.cpp:201
gjkepa2_impl::EPA::eStatus::_
_
Definition: btGjkEpa2.cpp:585
btGjkEpaSolver2::StackSizeRequirement
static int StackSizeRequirement()
Definition: btGjkEpa2.cpp:931
btGjkEpa2.h
btGjkEpaSolver2::Distance
static bool Distance(const btConvexShape *shape0, const btTransform &wtrs0, const btConvexShape *shape1, const btTransform &wtrs1, const btVector3 &guess, sResults &results)
Definition: btGjkEpa2.cpp:937
gjkepa2_impl::GJK::projectorigin
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, btScalar *w, U &m)
Definition: btGjkEpa2.cpp:457
gjkepa2_impl::EPA::eStatus::Valid
@ Valid
Definition: btGjkEpa2.cpp:586
gjkepa2_impl::GJK::sSimplex::p
btScalar p[4]
Definition: btGjkEpa2.cpp:165
btQuaternion
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:50
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257
gjkepa2_impl::MinkowskiDiff::Support
btVector3 Support(const btVector3 &d) const
Definition: btGjkEpa2.cpp:139
gjkepa2_impl::EPA::m_result
GJK::sSimplex m_result
Definition: btGjkEpa2.cpp:600
gjkepa2_impl::MinkowskiDiff::Support
btVector3 Support(const btVector3 &d, U index) const
Definition: btGjkEpa2.cpp:143
gjkepa2_impl::EPA::sFace::d
btScalar d
Definition: btGjkEpa2.cpp:562
gjkepa2_impl::GJK::m_simplex
sSimplex * m_simplex
Definition: btGjkEpa2.cpp:186
gjkepa2_impl::U1
unsigned char U1
Definition: btGjkEpa2.cpp:77
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
gjkepa2_impl::Initialize
static void Initialize(const btConvexShape *shape0, const btTransform &wtrs0, const btConvexShape *shape1, const btTransform &wtrs1, btGjkEpaSolver2::sResults &results, tShape &shape, bool withmargins)
Definition: btGjkEpa2.cpp:904
gjkepa2_impl::GJK::eStatus
Definition: btGjkEpa2.cpp:169
gjkepa2_impl::MinkowskiDiff::EnableMargin
void EnableMargin(bool enable)
Definition: btGjkEpa2.cpp:122
gjkepa2_impl::GJK
Definition: btGjkEpa2.cpp:156
gjkepa2_impl::MinkowskiDiff::m_toshape1
btMatrix3x3 m_toshape1
Definition: btGjkEpa2.cpp:83
gjkepa2_impl::EPA::m_nextsv
U m_nextsv
Definition: btGjkEpa2.cpp:605
gjkepa2_impl::EPA::getedgedist
bool getedgedist(sFace *face, sSV *a, sSV *b, btScalar &dist)
Definition: btGjkEpa2.cpp:769
EPA_MAX_FACES
#define EPA_MAX_FACES
Definition: btGjkEpa2.cpp:73
gjkepa2_impl::EPA::remove
static void remove(sList &list, sFace *face)
Definition: btGjkEpa2.cpp:629
btCross
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:918
gjkepa2_impl::GJK::m_simplices
sSimplex m_simplices[2]
Definition: btGjkEpa2.cpp:181
gjkepa2_impl::GJK::m_shape
tShape m_shape
Definition: btGjkEpa2.cpp:178
btGjkEpaSolver2::sResults
Definition: btGjkEpa2.h:34
EPA_MAX_VERTICES
#define EPA_MAX_VERTICES
Definition: btGjkEpa2.cpp:59
gjkepa2_impl::MinkowskiDiff::Support1
btVector3 Support1(const btVector3 &d) const
Definition: btGjkEpa2.cpp:133
gjkepa2_impl::EPA::eStatus::OutOfVertices
@ OutOfVertices
Definition: btGjkEpa2.cpp:592
gjkepa2_impl::EPA::sList::sList
sList()
Definition: btGjkEpa2.cpp:573
gjkepa2_impl::EPA::sList
Definition: btGjkEpa2.cpp:570
gjkepa2_impl::MinkowskiDiff
Definition: btGjkEpa2.cpp:81
gjkepa2_impl::EPA::expand
bool expand(U pass, sSV *w, sFace *f, U e, sHorizon &horizon)
Definition: btGjkEpa2.cpp:864
btGjkEpaSolver2::SignedDistance
static btScalar SignedDistance(const btVector3 &position, btScalar margin, const btConvexShape *shape, const btTransform &wtrs, sResults &results)
Definition: btGjkEpa2.cpp:1021
gjkepa2_impl::EPA::eStatus::AccuraryReached
@ AccuraryReached
Definition: btGjkEpa2.cpp:593
gjkepa2_impl::GJK::m_store
sSV m_store[4]
Definition: btGjkEpa2.cpp:182
GJK_MIN_DISTANCE
#define GJK_MIN_DISTANCE
Definition: btGjkEpa2.cpp:50
btMax
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
gjkepa2_impl::GJK::sSimplex::rank
U rank
Definition: btGjkEpa2.cpp:166
gjkepa2_impl::EPA::append
static void append(sList &list, sFace *face)
Definition: btGjkEpa2.cpp:621
gjkepa2_impl::MinkowskiDiff::m_toshape0
btTransform m_toshape0
Definition: btGjkEpa2.cpp:84
gjkepa2_impl::MinkowskiDiff::Ls
btVector3(btConvexShape::* Ls)(const btVector3 &) const
Definition: btGjkEpa2.cpp:88
GJK_MAX_ITERATIONS
#define GJK_MAX_ITERATIONS
Definition: btGjkEpa2.cpp:42
gjkepa2_impl::EPA::sList::root
sFace * root
Definition: btGjkEpa2.cpp:571
gjkepa2_impl::GJK::m_current
U m_current
Definition: btGjkEpa2.cpp:185
GJK_ACCURACY
#define GJK_ACCURACY
Definition: btGjkEpa2.cpp:49
gjkepa2_impl::EPA::sList::count
U count
Definition: btGjkEpa2.cpp:572
gjkepa2_impl::GJK::m_ray
btVector3 m_ray
Definition: btGjkEpa2.cpp:179
EPA_MAX_ITERATIONS
#define EPA_MAX_ITERATIONS
Definition: btGjkEpa2.cpp:60
btConvexShape::localGetSupportVertexWithoutMarginNonVirtual
btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3 &vec) const
Definition: btConvexShape.cpp:131
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:497
btGjkEpaSolver2::sResults::Separated
@ Separated
Definition: btGjkEpa2.h:37
gjkepa2_impl::EPA::m_hull
sList m_hull
Definition: btGjkEpa2.cpp:606
gjkepa2_impl::GJK::Initialize
void Initialize()
Definition: btGjkEpa2.cpp:193
gjkepa2_impl::EPA::sFace::l
sFace * l[2]
Definition: btGjkEpa2.cpp:565
gjkepa2_impl::EPA::findbest
sFace * findbest()
Definition: btGjkEpa2.cpp:849
gjkepa2_impl::EPA::newface
sFace * newface(sSV *a, sSV *b, sSV *c, bool forced)
Definition: btGjkEpa2.cpp:805
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
gjkepa2_impl::EPA::eStatus::OutOfFaces
@ OutOfFaces
Definition: btGjkEpa2.cpp:591
gjkepa2_impl::EPA::Evaluate
eStatus::_ Evaluate(GJK &gjk, const btVector3 &guess)
Definition: btGjkEpa2.cpp:648
gjkepa2_impl::EPA::sFace::f
sFace * f[3]
Definition: btGjkEpa2.cpp:564
gjkepa2_impl::EPA::m_depth
btScalar m_depth
Definition: btGjkEpa2.cpp:602
btConvexShape::getMarginNonVirtual
btScalar getMarginNonVirtual() const
Definition: btConvexShape.cpp:320
gjkepa2_impl::EPA::m_normal
btVector3 m_normal
Definition: btGjkEpa2.cpp:601
gjkepa2_impl::tShape
MinkowskiDiff tShape
Definition: btGjkEpa2.cpp:152
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
gjkepa2_impl::EPA::sSV
GJK::sSV sSV
Definition: btGjkEpa2.cpp:558
btGjkEpaSolver2::sResults::normal
btVector3 normal
Definition: btGjkEpa2.h:43
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
GJK_SIMPLEX4_EPS
#define GJK_SIMPLEX4_EPS
Definition: btGjkEpa2.cpp:56
gjkepa2_impl::EPA::sHorizon
Definition: btGjkEpa2.cpp:576
btGjkEpaSolver2::sResults::witnesses
btVector3 witnesses[2]
Definition: btGjkEpa2.h:42
gjkepa2_impl::GJK::GJK
GJK()
Definition: btGjkEpa2.cpp:189
EPA_ACCURACY
#define EPA_ACCURACY
Definition: btGjkEpa2.cpp:67
gjkepa2_impl::EPA::eStatus::Degenerated
@ Degenerated
Definition: btGjkEpa2.cpp:588
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:544
btSphereShape
The btSphereShape implements an implicit sphere, centered around a local origin with radius.
Definition: btSphereShape.h:25
gjkepa2_impl::EPA::sHorizon::sHorizon
sHorizon()
Definition: btGjkEpa2.cpp:580
gjkepa2_impl::EPA::m_sv_store
sSV m_sv_store[EPA_MAX_VERTICES]
Definition: btGjkEpa2.cpp:603
btMatrix3x3::transposeTimes
btMatrix3x3 transposeTimes(const btMatrix3x3 &m) const
Definition: btMatrix3x3.h:1106
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
gjkepa2_impl::EPA::m_fc_store
sFace m_fc_store[EPA_MAX_FACES]
Definition: btGjkEpa2.cpp:604
btSphereShape.h
gjkepa2_impl::GJK::sSV::d
btVector3 d
Definition: btGjkEpa2.cpp:160
gjkepa2_impl::EPA::m_stock
sList m_stock
Definition: btGjkEpa2.cpp:607
gjkepa2_impl::EPA::sFace::pass
U1 pass
Definition: btGjkEpa2.cpp:567
gjkepa2_impl::GJK::m_nfree
U m_nfree
Definition: btGjkEpa2.cpp:184
gjkepa2_impl::GJK::EncloseOrigin
bool EncloseOrigin()
Definition: btGjkEpa2.cpp:338
gjkepa2_impl::GJK::det
static btScalar det(const btVector3 &a, const btVector3 &b, const btVector3 &c)
Definition: btGjkEpa2.cpp:419
gjkepa2_impl::EPA::Initialize
void Initialize()
Definition: btGjkEpa2.cpp:637
gjkepa2_impl::MinkowskiDiff::MinkowskiDiff
MinkowskiDiff()
Definition: btGjkEpa2.cpp:91
gjkepa2_impl::EPA::eStatus::InvalidHull
@ InvalidHull
Definition: btGjkEpa2.cpp:590
gjkepa2_impl::EPA::sFace::e
U1 e[3]
Definition: btGjkEpa2.cpp:566
gjkepa2_impl::EPA::sHorizon::nf
U nf
Definition: btGjkEpa2.cpp:579
gjkepa2_impl::GJK::removevertice
void removevertice(sSimplex &simplex)
Definition: btGjkEpa2.cpp:409
gjkepa2_impl::EPA::eStatus::NonConvex
@ NonConvex
Definition: btGjkEpa2.cpp:589
btTransform::inverseTimes
btTransform inverseTimes(const btTransform &t) const
Return the inverse of this transform times the other transform.
Definition: btTransform.h:222
btSwap
void btSwap(T &a, T &b)
Definition: btScalar.h:643
btConvexShape
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:33
GJK_DUPLICATED_EPS
#define GJK_DUPLICATED_EPS
Definition: btGjkEpa2.cpp:51
btConvexShape::localGetSupportVertexNonVirtual
btVector3 localGetSupportVertexNonVirtual(const btVector3 &vec) const
Definition: btConvexShape.cpp:307
gjkepa2_impl::EPA::sFace
Definition: btGjkEpa2.cpp:560
gjkepa2_impl::MinkowskiDiff::m_shapes
const btConvexShape * m_shapes[2]
Definition: btGjkEpa2.cpp:82
btDot
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:890
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
gjkepa2_impl::GJK::sSV
Definition: btGjkEpa2.cpp:159
gjkepa2_impl::GJK::projectorigin
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, btScalar *w, U &m)
Definition: btGjkEpa2.cpp:425
gjkepa2_impl::GJK::eStatus::_
_
Definition: btGjkEpa2.cpp:171
gjkepa2_impl::EPA::sFace::c
sSV * c[3]
Definition: btGjkEpa2.cpp:563
gjkepa2_impl::EPA::bind
static void bind(sFace *fa, U ea, sFace *fb, U eb)
Definition: btGjkEpa2.cpp:614
gjkepa2_impl::GJK::sSimplex
Definition: btGjkEpa2.cpp:163
gjkepa2_impl::EPA::sHorizon::cf
sFace * cf
Definition: btGjkEpa2.cpp:577
gjkepa2_impl::U
unsigned int U
Definition: btGjkEpa2.cpp:76
gjkepa2_impl::GJK::eStatus::Failed
@ Failed
Definition: btGjkEpa2.cpp:174
gjkepa2_impl::EPA::eStatus::Touching
@ Touching
Definition: btGjkEpa2.cpp:587
gjkepa2_impl::MinkowskiDiff::Support0
btVector3 Support0(const btVector3 &d) const
Definition: btGjkEpa2.cpp:129
gjkepa2_impl::GJK::m_distance
btScalar m_distance
Definition: btGjkEpa2.cpp:180
gjkepa2_impl::EPA::m_status
eStatus::_ m_status
Definition: btGjkEpa2.cpp:599
EPA_PLANE_EPS
#define EPA_PLANE_EPS
Definition: btGjkEpa2.cpp:68
gjkepa2_impl::EPA::eStatus::Failed
@ Failed
Definition: btGjkEpa2.cpp:595
gjkepa2_impl::GJK::sSimplex::c
sSV * c[4]
Definition: btGjkEpa2.cpp:164
gjkepa2_impl::EPA::EPA
EPA()
Definition: btGjkEpa2.cpp:609
gjkepa2_impl::EPA::sFace::n
btVector3 n
Definition: btGjkEpa2.cpp:561
gjkepa2_impl
Definition: btGjkEpa2.cpp:38
gjkepa2_impl::GJK::projectorigin
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &d, btScalar *w, U &m)
Definition: btGjkEpa2.cpp:503
gjkepa2_impl::GJK::sSV::w
btVector3 w
Definition: btGjkEpa2.cpp:160
length
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:895
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
gjkepa2_impl::EPA
Definition: btGjkEpa2.cpp:556
gjkepa2_impl::GJK::eStatus::Valid
@ Valid
Definition: btGjkEpa2.cpp:172
btGjkEpaSolver2::sResults::status
enum btGjkEpaSolver2::sResults::eStatus status
gjkepa2_impl::GJK::m_free
sSV * m_free[4]
Definition: btGjkEpa2.cpp:183
gjkepa2_impl::EPA::sHorizon::ff
sFace * ff
Definition: btGjkEpa2.cpp:578
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition: btSoftBodyHelpers.cpp:94
gjkepa2_impl::EPA::eStatus
Definition: btGjkEpa2.cpp:583
gjkepa2_impl::EPA::eStatus::FallBack
@ FallBack
Definition: btGjkEpa2.cpp:594
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
gjkepa2_impl::GJK::m_status
eStatus::_ m_status
Definition: btGjkEpa2.cpp:187
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
gjkepa2_impl::GJK::appendvertice
void appendvertice(sSimplex &simplex, const btVector3 &v)
Definition: btGjkEpa2.cpp:413
btGjkEpaSolver2::sResults::distance
btScalar distance
Definition: btGjkEpa2.h:44
gjkepa2_impl::GJK::eStatus::Inside
@ Inside
Definition: btGjkEpa2.cpp:173
btGjkEpaSolver2::Penetration
static bool Penetration(const btConvexShape *shape0, const btTransform &wtrs0, const btConvexShape *shape1, const btTransform &wtrs1, const btVector3 &guess, sResults &results, bool usemargins=true)
Definition: btGjkEpa2.cpp:973
GJK_SIMPLEX3_EPS
#define GJK_SIMPLEX3_EPS
Definition: btGjkEpa2.cpp:55