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 http://continuousphysics.com/Bullet/
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
32
33
35{
37 {
39 {
40 Separated, /* Shapes doesnt penetrate */
41 Penetrating, /* Shapes are penetrating */
42 GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */
43 EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */
48 };
49
50
51};
52
53
54
55#if defined(DEBUG) || defined (_DEBUG)
56#include <stdio.h> //for debug printf
57#ifdef __SPU__
58#include <spu_printf.h>
59#define printf spu_printf
60#endif //__SPU__
61#endif
62
63
64
65 // Config
66
67 /* GJK */
68#define GJK_MAX_ITERATIONS 128
69#define GJK_ACCURARY ((btScalar)0.0001)
70#define GJK_MIN_DISTANCE ((btScalar)0.0001)
71#define GJK_DUPLICATED_EPS ((btScalar)0.0001)
72#define GJK_SIMPLEX2_EPS ((btScalar)0.0)
73#define GJK_SIMPLEX3_EPS ((btScalar)0.0)
74#define GJK_SIMPLEX4_EPS ((btScalar)0.0)
75
76 /* EPA */
77#define EPA_MAX_VERTICES 64
78#define EPA_MAX_FACES (EPA_MAX_VERTICES*2)
79#define EPA_MAX_ITERATIONS 255
80#define EPA_ACCURACY ((btScalar)0.0001)
81#define EPA_FALLBACK (10*EPA_ACCURACY)
82#define EPA_PLANE_EPS ((btScalar)0.00001)
83#define EPA_INSIDE_EPS ((btScalar)0.01)
84
85
86 // Shorthands
87 typedef unsigned int U;
88 typedef unsigned char U1;
89
90 // MinkowskiDiff
91 template <typename btConvexTemplate>
93 {
94 const btConvexTemplate* m_convexAPtr;
95 const btConvexTemplate* m_convexBPtr;
96
99
101
102
103 MinkowskiDiff(const btConvexTemplate& a, const btConvexTemplate& b)
104 :m_convexAPtr(&a),
105 m_convexBPtr(&b)
106 {
107 }
108
109 void EnableMargin(bool enable)
110 {
111 m_enableMargin = enable;
112 }
113 inline btVector3 Support0(const btVector3& d) const
114 {
115 return m_convexAPtr->getLocalSupportWithMargin(d);
116 }
117 inline btVector3 Support1(const btVector3& d) const
118 {
119 return m_toshape0*m_convexBPtr->getLocalSupportWithMargin(m_toshape1*d);
120 }
121
122
123 inline btVector3 Support(const btVector3& d) const
124 {
125 return(Support0(d)-Support1(-d));
126 }
127 btVector3 Support(const btVector3& d,U index) const
128 {
129 if(index)
130 return(Support1(d));
131 else
132 return(Support0(d));
133 }
134 };
135
137{
142
143 // GJK
144 template <typename btConvexTemplate>
145 struct GJK
146 {
147 /* Types */
148 struct sSV
149 {
151 };
152 struct sSimplex
153 {
154 sSV* c[4];
157 };
158
159 /* Fields */
160
171 /* Methods */
172
173 GJK(const btConvexTemplate& a, const btConvexTemplate& b)
174 :m_shape(a,b)
175 {
176 Initialize();
177 }
179 {
180 m_ray = btVector3(0,0,0);
181 m_nfree = 0;
183 m_current = 0;
184 m_distance = 0;
185 }
187 {
188 U iterations=0;
189 btScalar sqdist=0;
190 btScalar alpha=0;
191 btVector3 lastw[4];
192 U clastw=0;
193 /* Initialize solver */
194 m_free[0] = &m_store[0];
195 m_free[1] = &m_store[1];
196 m_free[2] = &m_store[2];
197 m_free[3] = &m_store[3];
198 m_nfree = 4;
199 m_current = 0;
201 m_shape = shapearg;
202 m_distance = 0;
203 /* Initialize simplex */
204 m_simplices[0].rank = 0;
205 m_ray = guess;
206 const btScalar sqrl= m_ray.length2();
207 appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0));
208 m_simplices[0].p[0] = 1;
209 m_ray = m_simplices[0].c[0]->w;
210 sqdist = sqrl;
211 lastw[0] =
212 lastw[1] =
213 lastw[2] =
214 lastw[3] = m_ray;
215 /* Loop */
216 do {
217 const U next=1-m_current;
219 sSimplex& ns=m_simplices[next];
220 /* Check zero */
221 const btScalar rl=m_ray.length();
222 if(rl<GJK_MIN_DISTANCE)
223 {/* Touching or inside */
225 break;
226 }
227 /* Append new vertice in -'v' direction */
228 appendvertice(cs,-m_ray);
229 const btVector3& w=cs.c[cs.rank-1]->w;
230 bool found=false;
231 for(U i=0;i<4;++i)
232 {
233 if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
234 { found=true;break; }
235 }
236 if(found)
237 {/* Return old simplex */
239 break;
240 }
241 else
242 {/* Update lastw */
243 lastw[clastw=(clastw+1)&3]=w;
244 }
245 /* Check for termination */
246 const btScalar omega=btDot(m_ray,w)/rl;
247 alpha=btMax(omega,alpha);
248 if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
249 {/* Return old simplex */
251 break;
252 }
253 /* Reduce simplex */
254 btScalar weights[4];
255 U mask=0;
256 switch(cs.rank)
257 {
258 case 2: sqdist=projectorigin( cs.c[0]->w,
259 cs.c[1]->w,
260 weights,mask);break;
261 case 3: sqdist=projectorigin( cs.c[0]->w,
262 cs.c[1]->w,
263 cs.c[2]->w,
264 weights,mask);break;
265 case 4: sqdist=projectorigin( cs.c[0]->w,
266 cs.c[1]->w,
267 cs.c[2]->w,
268 cs.c[3]->w,
269 weights,mask);break;
270 }
271 if(sqdist>=0)
272 {/* Valid */
273 ns.rank = 0;
274 m_ray = btVector3(0,0,0);
275 m_current = next;
276 for(U i=0,ni=cs.rank;i<ni;++i)
277 {
278 if(mask&(1<<i))
279 {
280 ns.c[ns.rank] = cs.c[i];
281 ns.p[ns.rank++] = weights[i];
282 m_ray += cs.c[i]->w*weights[i];
283 }
284 else
285 {
286 m_free[m_nfree++] = cs.c[i];
287 }
288 }
289 if(mask==15) m_status=eGjkInside;
290 }
291 else
292 {/* Return old simplex */
294 break;
295 }
297 } while(m_status==eGjkValid);
299 switch(m_status)
300 {
301 case eGjkValid: m_distance=m_ray.length();break;
302 case eGjkInside: m_distance=0;break;
303 default:
304 {
305 }
306 }
307 return(m_status);
308 }
310 {
311 switch(m_simplex->rank)
312 {
313 case 1:
314 {
315 for(U i=0;i<3;++i)
316 {
317 btVector3 axis=btVector3(0,0,0);
318 axis[i]=1;
319 appendvertice(*m_simplex, axis);
320 if(EncloseOrigin()) return(true);
322 appendvertice(*m_simplex,-axis);
323 if(EncloseOrigin()) return(true);
325 }
326 }
327 break;
328 case 2:
329 {
330 const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w;
331 for(U i=0;i<3;++i)
332 {
333 btVector3 axis=btVector3(0,0,0);
334 axis[i]=1;
335 const btVector3 p=btCross(d,axis);
336 if(p.length2()>0)
337 {
339 if(EncloseOrigin()) return(true);
342 if(EncloseOrigin()) return(true);
344 }
345 }
346 }
347 break;
348 case 3:
349 {
350 const btVector3 n=btCross(m_simplex->c[1]->w-m_simplex->c[0]->w,
351 m_simplex->c[2]->w-m_simplex->c[0]->w);
352 if(n.length2()>0)
353 {
355 if(EncloseOrigin()) return(true);
358 if(EncloseOrigin()) return(true);
360 }
361 }
362 break;
363 case 4:
364 {
365 if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w,
366 m_simplex->c[1]->w-m_simplex->c[3]->w,
367 m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
368 return(true);
369 }
370 break;
371 }
372 return(false);
373 }
374 /* Internals */
375 void getsupport(const btVector3& d,sSV& sv) const
376 {
377 sv.d = d/d.length();
378 sv.w = m_shape.Support(sv.d);
379 }
380 void removevertice(sSimplex& simplex)
381 {
382 m_free[m_nfree++]=simplex.c[--simplex.rank];
383 }
384 void appendvertice(sSimplex& simplex,const btVector3& v)
385 {
386 simplex.p[simplex.rank]=0;
387 simplex.c[simplex.rank]=m_free[--m_nfree];
388 getsupport(v,*simplex.c[simplex.rank++]);
389 }
390 static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c)
391 {
392 return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()-
393 a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+
394 a.x()*b.y()*c.z()-a.z()*b.y()*c.x());
395 }
397 const btVector3& b,
398 btScalar* w,U& m)
399 {
400 const btVector3 d=b-a;
401 const btScalar l=d.length2();
402 if(l>GJK_SIMPLEX2_EPS)
403 {
404 const btScalar t(l>0?-btDot(a,d)/l:0);
405 if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); }
406 else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); }
407 else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
408 }
409 return(-1);
410 }
412 const btVector3& b,
413 const btVector3& c,
414 btScalar* w,U& m)
415 {
416 static const U imd3[]={1,2,0};
417 const btVector3* vt[]={&a,&b,&c};
418 const btVector3 dl[]={a-b,b-c,c-a};
419 const btVector3 n=btCross(dl[0],dl[1]);
420 const btScalar l=n.length2();
421 if(l>GJK_SIMPLEX3_EPS)
422 {
423 btScalar mindist=-1;
424 btScalar subw[2]={0.f,0.f};
425 U subm(0);
426 for(U i=0;i<3;++i)
427 {
428 if(btDot(*vt[i],btCross(dl[i],n))>0)
429 {
430 const U j=imd3[i];
431 const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm));
432 if((mindist<0)||(subd<mindist))
433 {
434 mindist = subd;
435 m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
436 w[i] = subw[0];
437 w[j] = subw[1];
438 w[imd3[j]] = 0;
439 }
440 }
441 }
442 if(mindist<0)
443 {
444 const btScalar d=btDot(a,n);
445 const btScalar s=btSqrt(l);
446 const btVector3 p=n*(d/l);
447 mindist = p.length2();
448 m = 7;
449 w[0] = (btCross(dl[1],b-p)).length()/s;
450 w[1] = (btCross(dl[2],c-p)).length()/s;
451 w[2] = 1-(w[0]+w[1]);
452 }
453 return(mindist);
454 }
455 return(-1);
456 }
458 const btVector3& b,
459 const btVector3& c,
460 const btVector3& d,
461 btScalar* w,U& m)
462 {
463 static const U imd3[]={1,2,0};
464 const btVector3* vt[]={&a,&b,&c,&d};
465 const btVector3 dl[]={a-d,b-d,c-d};
466 const btScalar vl=det(dl[0],dl[1],dl[2]);
467 const bool ng=(vl*btDot(a,btCross(b-c,a-b)))<=0;
468 if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
469 {
470 btScalar mindist=-1;
471 btScalar subw[3]={0.f,0.f,0.f};
472 U subm(0);
473 for(U i=0;i<3;++i)
474 {
475 const U j=imd3[i];
476 const btScalar s=vl*btDot(d,btCross(dl[i],dl[j]));
477 if(s>0)
478 {
479 const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
480 if((mindist<0)||(subd<mindist))
481 {
482 mindist = subd;
483 m = static_cast<U>((subm&1?1<<i:0)+
484 (subm&2?1<<j:0)+
485 (subm&4?8:0));
486 w[i] = subw[0];
487 w[j] = subw[1];
488 w[imd3[j]] = 0;
489 w[3] = subw[2];
490 }
491 }
492 }
493 if(mindist<0)
494 {
495 mindist = 0;
496 m = 15;
497 w[0] = det(c,b,d)/vl;
498 w[1] = det(a,c,d)/vl;
499 w[2] = det(b,a,d)/vl;
500 w[3] = 1-(w[0]+w[1]+w[2]);
501 }
502 return(mindist);
503 }
504 return(-1);
505 }
506 };
507
508
510{
522
523
524 // EPA
525template <typename btConvexTemplate>
526 struct EPA
527 {
528 /* Types */
529
530 struct sFace
531 {
535 sFace* f[3];
536 sFace* l[2];
537 U1 e[3];
539 };
540 struct sList
541 {
544 sList() : root(0),count(0) {}
545 };
546 struct sHorizon
547 {
551 sHorizon() : cf(0),ff(0),nf(0) {}
552 };
553
554 /* Fields */
564 /* Methods */
566 {
567 Initialize();
568 }
569
570
571 static inline void bind(sFace* fa,U ea,sFace* fb,U eb)
572 {
573 fa->e[ea]=(U1)eb;fa->f[ea]=fb;
574 fb->e[eb]=(U1)ea;fb->f[eb]=fa;
575 }
576 static inline void append(sList& list,sFace* face)
577 {
578 face->l[0] = 0;
579 face->l[1] = list.root;
580 if(list.root) list.root->l[0]=face;
581 list.root = face;
582 ++list.count;
583 }
584 static inline void remove(sList& list,sFace* face)
585 {
586 if(face->l[1]) face->l[1]->l[0]=face->l[0];
587 if(face->l[0]) face->l[0]->l[1]=face->l[1];
588 if(face==list.root) list.root=face->l[1];
589 --list.count;
590 }
591
592
594 {
596 m_normal = btVector3(0,0,0);
597 m_depth = 0;
598 m_nextsv = 0;
599 for(U i=0;i<EPA_MAX_FACES;++i)
600 {
602 }
603 }
605 {
606 typename GJK<btConvexTemplate>::sSimplex& simplex=*gjk.m_simplex;
607 if((simplex.rank>1)&&gjk.EncloseOrigin())
608 {
609
610 /* Clean up */
611 while(m_hull.root)
612 {
613 sFace* f = m_hull.root;
614 remove(m_hull,f);
615 append(m_stock,f);
616 }
618 m_nextsv = 0;
619 /* Orient simplex */
620 if(gjk.det( simplex.c[0]->w-simplex.c[3]->w,
621 simplex.c[1]->w-simplex.c[3]->w,
622 simplex.c[2]->w-simplex.c[3]->w)<0)
623 {
624 btSwap(simplex.c[0],simplex.c[1]);
625 btSwap(simplex.p[0],simplex.p[1]);
626 }
627 /* Build initial hull */
628 sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
629 newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
630 newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
631 newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
632 if(m_hull.count==4)
633 {
634 sFace* best=findbest();
635 sFace outer=*best;
636 U pass=0;
637 U iterations=0;
638 bind(tetra[0],0,tetra[1],0);
639 bind(tetra[0],1,tetra[2],0);
640 bind(tetra[0],2,tetra[3],0);
641 bind(tetra[1],1,tetra[3],2);
642 bind(tetra[1],2,tetra[2],1);
643 bind(tetra[2],2,tetra[3],1);
645 for(;iterations<EPA_MAX_ITERATIONS;++iterations)
646 {
648 {
649 sHorizon horizon;
651 bool valid=true;
652 best->pass = (U1)(++pass);
653 gjk.getsupport(best->n,*w);
654 const btScalar wdist=btDot(best->n,w->w)-best->d;
655 if(wdist>EPA_ACCURACY)
656 {
657 for(U j=0;(j<3)&&valid;++j)
658 {
659 valid&=expand( pass,w,
660 best->f[j],best->e[j],
661 horizon);
662 }
663 if(valid&&(horizon.nf>=3))
664 {
665 bind(horizon.cf,1,horizon.ff,2);
666 remove(m_hull,best);
667 append(m_stock,best);
668 best=findbest();
669 outer=*best;
670 } else { m_status=eEpaInvalidHull;break; }
671 } else { m_status=eEpaAccuraryReached;break; }
672 } else { m_status=eEpaOutOfVertices;break; }
673 }
674 const btVector3 projection=outer.n*outer.d;
675 m_normal = outer.n;
676 m_depth = outer.d;
677 m_result.rank = 3;
678 m_result.c[0] = outer.c[0];
679 m_result.c[1] = outer.c[1];
680 m_result.c[2] = outer.c[2];
681 m_result.p[0] = btCross( outer.c[1]->w-projection,
682 outer.c[2]->w-projection).length();
683 m_result.p[1] = btCross( outer.c[2]->w-projection,
684 outer.c[0]->w-projection).length();
685 m_result.p[2] = btCross( outer.c[0]->w-projection,
686 outer.c[1]->w-projection).length();
687 const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
688 m_result.p[0] /= sum;
689 m_result.p[1] /= sum;
690 m_result.p[2] /= sum;
691 return(m_status);
692 }
693 }
694 /* Fallback */
696 m_normal = -guess;
697 const btScalar nl=m_normal.length();
698 if(nl>0)
699 m_normal = m_normal/nl;
700 else
701 m_normal = btVector3(1,0,0);
702 m_depth = 0;
703 m_result.rank=1;
704 m_result.c[0]=simplex.c[0];
705 m_result.p[0]=1;
706 return(m_status);
707 }
709 {
710 const btVector3 ba = b->w - a->w;
711 const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane
712 const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required
713
714 if(a_dot_nab < 0)
715 {
716 // Outside of edge a->b
717
718 const btScalar ba_l2 = ba.length2();
719 const btScalar a_dot_ba = btDot(a->w, ba);
720 const btScalar b_dot_ba = btDot(b->w, ba);
721
722 if(a_dot_ba > 0)
723 {
724 // Pick distance vertex a
725 dist = a->w.length();
726 }
727 else if(b_dot_ba < 0)
728 {
729 // Pick distance vertex b
730 dist = b->w.length();
731 }
732 else
733 {
734 // Pick distance to edge a->b
735 const btScalar a_dot_b = btDot(a->w, b->w);
736 dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0));
737 }
738
739 return true;
740 }
741
742 return false;
743 }
745 {
746 if(m_stock.root)
747 {
748 sFace* face=m_stock.root;
749 remove(m_stock,face);
750 append(m_hull,face);
751 face->pass = 0;
752 face->c[0] = a;
753 face->c[1] = b;
754 face->c[2] = c;
755 face->n = btCross(b->w-a->w,c->w-a->w);
756 const btScalar l=face->n.length();
757 const bool v=l>EPA_ACCURACY;
758
759 if(v)
760 {
761 if(!(getedgedist(face, a, b, face->d) ||
762 getedgedist(face, b, c, face->d) ||
763 getedgedist(face, c, a, face->d)))
764 {
765 // Origin projects to the interior of the triangle
766 // Use distance to triangle plane
767 face->d = btDot(a->w, face->n) / l;
768 }
769
770 face->n /= l;
771 if(forced || (face->d >= -EPA_PLANE_EPS))
772 {
773 return face;
774 }
775 else
777 }
778 else
780
781 remove(m_hull, face);
782 append(m_stock, face);
783 return 0;
784
785 }
787 return 0;
788 }
790 {
791 sFace* minf=m_hull.root;
792 btScalar mind=minf->d*minf->d;
793 for(sFace* f=minf->l[1];f;f=f->l[1])
794 {
795 const btScalar sqd=f->d*f->d;
796 if(sqd<mind)
797 {
798 minf=f;
799 mind=sqd;
800 }
801 }
802 return(minf);
803 }
804 bool expand(U pass,typename GJK<btConvexTemplate>::sSV* w,sFace* f,U e,sHorizon& horizon)
805 {
806 static const U i1m3[]={1,2,0};
807 static const U i2m3[]={2,0,1};
808 if(f->pass!=pass)
809 {
810 const U e1=i1m3[e];
811 if((btDot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
812 {
813 sFace* nf=newface(f->c[e1],f->c[e],w,false);
814 if(nf)
815 {
816 bind(nf,0,f,e);
817 if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
818 horizon.cf=nf;
819 ++horizon.nf;
820 return(true);
821 }
822 }
823 else
824 {
825 const U e2=i2m3[e];
826 f->pass = (U1)pass;
827 if( expand(pass,w,f->f[e1],f->e[e1],horizon)&&
828 expand(pass,w,f->f[e2],f->e[e2],horizon))
829 {
830 remove(m_hull,f);
831 append(m_stock,f);
832 return(true);
833 }
834 }
835 }
836 return(false);
837 }
838
839 };
840
841 template <typename btConvexTemplate>
842 static void Initialize( const btConvexTemplate& a, const btConvexTemplate& b,
845 {
846 /* Results */
847 results.witnesses[0] =
848 results.witnesses[1] = btVector3(0,0,0);
850 /* Shape */
851
852 shape.m_toshape1 = b.getWorldTransform().getBasis().transposeTimes(a.getWorldTransform().getBasis());
853 shape.m_toshape0 = a.getWorldTransform().inverseTimes(b.getWorldTransform());
854
855 }
856
857
858//
859// Api
860//
861
862
863
864//
865template <typename btConvexTemplate>
866bool btGjkEpaSolver3_Distance(const btConvexTemplate& a, const btConvexTemplate& b,
867 const btVector3& guess,
869{
871 Initialize(a,b,results,shape);
872 GJK<btConvexTemplate> gjk(a,b);
873 eGjkStatus gjk_status=gjk.Evaluate(shape,guess);
874 if(gjk_status==eGjkValid)
875 {
876 btVector3 w0=btVector3(0,0,0);
877 btVector3 w1=btVector3(0,0,0);
878 for(U i=0;i<gjk.m_simplex->rank;++i)
879 {
880 const btScalar p=gjk.m_simplex->p[i];
881 w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
882 w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
883 }
884 results.witnesses[0] = a.getWorldTransform()*w0;
885 results.witnesses[1] = a.getWorldTransform()*w1;
886 results.normal = w0-w1;
887 results.distance = results.normal.length();
888 results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1;
889 return(true);
890 }
891 else
892 {
893 results.status = gjk_status==eGjkInside?
896 return(false);
897 }
898}
899
900
901template <typename btConvexTemplate>
902bool btGjkEpaSolver3_Penetration(const btConvexTemplate& a,
903 const btConvexTemplate& b,
904 const btVector3& guess,
906{
908 Initialize(a,b,results,shape);
909 GJK<btConvexTemplate> gjk(a,b);
910 eGjkStatus gjk_status=gjk.Evaluate(shape,-guess);
911 switch(gjk_status)
912 {
913 case eGjkInside:
914 {
916 eEpaStatus epa_status=epa.Evaluate(gjk,-guess);
917 if(epa_status!=eEpaFailed)
918 {
919 btVector3 w0=btVector3(0,0,0);
920 for(U i=0;i<epa.m_result.rank;++i)
921 {
922 w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
923 }
925 results.witnesses[0] = a.getWorldTransform()*w0;
926 results.witnesses[1] = a.getWorldTransform()*(w0-epa.m_normal*epa.m_depth);
927 results.normal = -epa.m_normal;
928 results.distance = -epa.m_depth;
929 return(true);
931 }
932 break;
933 case eGjkFailed:
935 break;
936 default:
937 {
938 }
939 }
940 return(false);
941}
942
943#if 0
944int btComputeGjkEpaPenetration2(const btCollisionDescription& colDesc, btDistanceInfo* distInfo)
945{
947 btVector3 guess = colDesc.m_firstDir;
948
949 bool res = btGjkEpaSolver3::Penetration(colDesc.m_objA,colDesc.m_objB,
950 colDesc.m_transformA,colDesc.m_transformB,
951 colDesc.m_localSupportFuncA,colDesc.m_localSupportFuncB,
952 guess,
953 results);
954 if (res)
955 {
956 if ((results.status==btGjkEpaSolver3::sResults::Penetrating) || results.status==GJK::eStatus::Inside)
957 {
958 //normal could be 'swapped'
959
960 distInfo->m_distance = results.distance;
961 distInfo->m_normalBtoA = results.normal;
962 btVector3 tmpNormalInB = results.witnesses[1]-results.witnesses[0];
963 btScalar lenSqr = tmpNormalInB.length2();
964 if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
965 {
966 tmpNormalInB = results.normal;
967 lenSqr = results.normal.length2();
968 }
969
970 if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
971 {
972 tmpNormalInB /= btSqrt(lenSqr);
973 btScalar distance2 = -(results.witnesses[0]-results.witnesses[1]).length();
974 //only replace valid penetrations when the result is deeper (check)
975 //if ((distance2 < results.distance))
976 {
977 distInfo->m_distance = distance2;
978 distInfo->m_pointOnA= results.witnesses[0];
979 distInfo->m_pointOnB= results.witnesses[1];
980 distInfo->m_normalBtoA= tmpNormalInB;
981 return 0;
982 }
983 }
984 }
985
986 }
987
988 return -1;
989}
990#endif
991
992template <typename btConvexTemplate, typename btDistanceInfoTemplate>
993int btComputeGjkDistance(const btConvexTemplate& a, const btConvexTemplate& b,
994 const btGjkCollisionDescription& colDesc, btDistanceInfoTemplate* distInfo)
995{
997 btVector3 guess = colDesc.m_firstDir;
998
999 bool isSeparated = btGjkEpaSolver3_Distance( a,b,
1000 guess,
1001 results);
1002 if (isSeparated)
1003 {
1004 distInfo->m_distance = results.distance;
1005 distInfo->m_pointOnA= results.witnesses[0];
1006 distInfo->m_pointOnB= results.witnesses[1];
1007 distInfo->m_normalBtoA= results.normal;
1008 return 0;
1009 }
1010
1011 return -1;
1012}
1013
1014/* Symbols cleanup */
1015
1016#undef GJK_MAX_ITERATIONS
1017#undef GJK_ACCURARY
1018#undef GJK_MIN_DISTANCE
1019#undef GJK_DUPLICATED_EPS
1020#undef GJK_SIMPLEX2_EPS
1021#undef GJK_SIMPLEX3_EPS
1022#undef GJK_SIMPLEX4_EPS
1023
1024#undef EPA_MAX_VERTICES
1025#undef EPA_MAX_FACES
1026#undef EPA_MAX_ITERATIONS
1027#undef EPA_ACCURACY
1028#undef EPA_FALLBACK
1029#undef EPA_PLANE_EPS
1030#undef EPA_INSIDE_EPS
1031
1032
1033
1034#endif //BT_GJK_EPA3_H
1035
#define EPA_ACCURACY
Definition: btGjkEpa3.h:80
int btComputeGjkDistance(const btConvexTemplate &a, const btConvexTemplate &b, const btGjkCollisionDescription &colDesc, btDistanceInfoTemplate *distInfo)
Definition: btGjkEpa3.h:993
unsigned int U
Definition: btGjkEpa3.h:87
#define GJK_ACCURARY
Definition: btGjkEpa3.h:69
#define EPA_MAX_FACES
Definition: btGjkEpa3.h:78
static void Initialize(const btConvexTemplate &a, const btConvexTemplate &b, btGjkEpaSolver3::sResults &results, MinkowskiDiff< btConvexTemplate > &shape)
Definition: btGjkEpa3.h:842
#define GJK_MIN_DISTANCE
Definition: btGjkEpa3.h:70
#define GJK_SIMPLEX4_EPS
Definition: btGjkEpa3.h:74
#define EPA_PLANE_EPS
Definition: btGjkEpa3.h:82
#define GJK_DUPLICATED_EPS
Definition: btGjkEpa3.h:71
bool btGjkEpaSolver3_Penetration(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
Definition: btGjkEpa3.h:902
eEpaStatus
Definition: btGjkEpa3.h:510
@ eEpaFailed
Definition: btGjkEpa3.h:520
@ eEpaFallBack
Definition: btGjkEpa3.h:519
@ eEpaDegenerated
Definition: btGjkEpa3.h:513
@ eEpaInvalidHull
Definition: btGjkEpa3.h:515
@ eEpaNonConvex
Definition: btGjkEpa3.h:514
@ eEpaOutOfVertices
Definition: btGjkEpa3.h:517
@ eEpaTouching
Definition: btGjkEpa3.h:512
@ eEpaAccuraryReached
Definition: btGjkEpa3.h:518
@ eEpaValid
Definition: btGjkEpa3.h:511
@ eEpaOutOfFaces
Definition: btGjkEpa3.h:516
#define EPA_MAX_VERTICES
Definition: btGjkEpa3.h:77
#define GJK_SIMPLEX2_EPS
Definition: btGjkEpa3.h:72
#define GJK_MAX_ITERATIONS
Definition: btGjkEpa3.h:68
#define GJK_SIMPLEX3_EPS
Definition: btGjkEpa3.h:73
bool btGjkEpaSolver3_Distance(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
Definition: btGjkEpa3.h:866
unsigned char U1
Definition: btGjkEpa3.h:88
eGjkStatus
Definition: btGjkEpa3.h:137
@ eGjkValid
Definition: btGjkEpa3.h:138
@ eGjkInside
Definition: btGjkEpa3.h:139
@ eGjkFailed
Definition: btGjkEpa3.h:140
#define EPA_MAX_ITERATIONS
Definition: btGjkEpa3.h:79
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:29
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:886
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btScalar btSqrt(btScalar y)
Definition: btScalar.h:444
btScalar btFabs(btScalar x)
Definition: btScalar.h:475
#define SIMD_EPSILON
Definition: btScalar.h:521
void btSwap(T &a, T &b)
Definition: btScalar.h:621
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:901
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:931
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
btMatrix3x3 transposeTimes(const btMatrix3x3 &m) const
Definition: btMatrix3x3.h:1016
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btTransform inverseTimes(const btTransform &t) const
Return the inverse of this transform times the other transform.
Definition: btTransform.h:230
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
const btScalar & z() const
Return the z value.
Definition: btVector3.h:591
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
const btScalar & x() const
Return the x value.
Definition: btVector3.h:587
const btScalar & y() const
Return the y value.
Definition: btVector3.h:589
sFace * l[2]
Definition: btGjkEpa3.h:536
sFace * f[3]
Definition: btGjkEpa3.h:535
GJK< btConvexTemplate >::sSV * c[3]
Definition: btGjkEpa3.h:534
btScalar d
Definition: btGjkEpa3.h:533
U1 e[3]
Definition: btGjkEpa3.h:537
btVector3 n
Definition: btGjkEpa3.h:532
sFace * cf
Definition: btGjkEpa3.h:548
sFace * ff
Definition: btGjkEpa3.h:549
sFace * root
Definition: btGjkEpa3.h:542
Definition: btGjkEpa3.h:527
bool getedgedist(sFace *face, typename GJK< btConvexTemplate >::sSV *a, typename GJK< btConvexTemplate >::sSV *b, btScalar &dist)
Definition: btGjkEpa3.h:708
bool expand(U pass, typename GJK< btConvexTemplate >::sSV *w, sFace *f, U e, sHorizon &horizon)
Definition: btGjkEpa3.h:804
static void append(sList &list, sFace *face)
Definition: btGjkEpa3.h:576
sFace * newface(typename GJK< btConvexTemplate >::sSV *a, typename GJK< btConvexTemplate >::sSV *b, typename GJK< btConvexTemplate >::sSV *c, bool forced)
Definition: btGjkEpa3.h:744
GJK< btConvexTemplate >::sSimplex m_result
Definition: btGjkEpa3.h:556
GJK< btConvexTemplate >::sSV m_sv_store[EPA_MAX_VERTICES]
Definition: btGjkEpa3.h:559
sList m_stock
Definition: btGjkEpa3.h:563
static void remove(sList &list, sFace *face)
Definition: btGjkEpa3.h:584
eEpaStatus Evaluate(GJK< btConvexTemplate > &gjk, const btVector3 &guess)
Definition: btGjkEpa3.h:604
btScalar m_depth
Definition: btGjkEpa3.h:558
U m_nextsv
Definition: btGjkEpa3.h:561
sList m_hull
Definition: btGjkEpa3.h:562
static void bind(sFace *fa, U ea, sFace *fb, U eb)
Definition: btGjkEpa3.h:571
btVector3 m_normal
Definition: btGjkEpa3.h:557
sFace m_fc_store[EPA_MAX_FACES]
Definition: btGjkEpa3.h:560
void Initialize()
Definition: btGjkEpa3.h:593
eEpaStatus m_status
Definition: btGjkEpa3.h:555
EPA()
Definition: btGjkEpa3.h:565
sFace * findbest()
Definition: btGjkEpa3.h:789
btVector3 w
Definition: btGjkEpa3.h:150
btVector3 d
Definition: btGjkEpa3.h:150
sSV * c[4]
Definition: btGjkEpa3.h:154
btScalar p[4]
Definition: btGjkEpa3.h:155
Definition: btGjkEpa3.h:146
U m_nfree
Definition: btGjkEpa3.h:167
eGjkStatus Evaluate(const MinkowskiDiff< btConvexTemplate > &shapearg, const btVector3 &guess)
Definition: btGjkEpa3.h:186
void getsupport(const btVector3 &d, sSV &sv) const
Definition: btGjkEpa3.h:375
MinkowskiDiff< btConvexTemplate > m_shape
Definition: btGjkEpa3.h:161
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &d, btScalar *w, U &m)
Definition: btGjkEpa3.h:457
U m_current
Definition: btGjkEpa3.h:168
bool EncloseOrigin()
Definition: btGjkEpa3.h:309
sSV * m_free[4]
Definition: btGjkEpa3.h:166
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, btScalar *w, U &m)
Definition: btGjkEpa3.h:396
sSV m_store[4]
Definition: btGjkEpa3.h:165
sSimplex * m_simplex
Definition: btGjkEpa3.h:169
static btScalar det(const btVector3 &a, const btVector3 &b, const btVector3 &c)
Definition: btGjkEpa3.h:390
void Initialize()
Definition: btGjkEpa3.h:178
GJK(const btConvexTemplate &a, const btConvexTemplate &b)
Definition: btGjkEpa3.h:173
sSimplex m_simplices[2]
Definition: btGjkEpa3.h:164
btVector3 m_ray
Definition: btGjkEpa3.h:162
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, btScalar *w, U &m)
Definition: btGjkEpa3.h:411
void removevertice(sSimplex &simplex)
Definition: btGjkEpa3.h:380
eGjkStatus m_status
Definition: btGjkEpa3.h:170
btScalar m_distance
Definition: btGjkEpa3.h:163
void appendvertice(sSimplex &simplex, const btVector3 &v)
Definition: btGjkEpa3.h:384
btVector3 Support1(const btVector3 &d) const
Definition: btGjkEpa3.h:117
btVector3 Support0(const btVector3 &d) const
Definition: btGjkEpa3.h:113
void EnableMargin(bool enable)
Definition: btGjkEpa3.h:109
btVector3 Support(const btVector3 &d) const
Definition: btGjkEpa3.h:123
btVector3 Support(const btVector3 &d, U index) const
Definition: btGjkEpa3.h:127
MinkowskiDiff(const btConvexTemplate &a, const btConvexTemplate &b)
Definition: btGjkEpa3.h:103
btTransform m_toshape0
Definition: btGjkEpa3.h:98
const btConvexTemplate * m_convexBPtr
Definition: btGjkEpa3.h:95
const btConvexTemplate * m_convexAPtr
Definition: btGjkEpa3.h:94
bool m_enableMargin
Definition: btGjkEpa3.h:100
btMatrix3x3 m_toshape1
Definition: btGjkEpa3.h:97
enum btGjkEpaSolver3::sResults::eStatus status
btVector3 witnesses[2]
Definition: btGjkEpa3.h:45