Bullet Collision Detection & Physics Library
btGjkEpa2.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2008 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/*
23GJK-EPA collision solver by Nathanael Presson, 2008
24*/
27#include "btGjkEpa2.h"
28
29#if defined(DEBUG) || defined (_DEBUG)
30#include <stdio.h> //for debug printf
31#ifdef __SPU__
32#include <spu_printf.h>
33#define printf spu_printf
34#endif //__SPU__
35#endif
36
37namespace gjkepa2_impl
38{
39
40 // Config
41
42 /* GJK */
43#define GJK_MAX_ITERATIONS 128
44
45#ifdef BT_USE_DOUBLE_PRECISION
46 #define GJK_ACCURACY ((btScalar)1e-12)
47 #define GJK_MIN_DISTANCE ((btScalar)1e-12)
48 #define GJK_DUPLICATED_EPS ((btScalar)1e-12)
49#else
50 #define GJK_ACCURACY ((btScalar)0.0001)
51 #define GJK_MIN_DISTANCE ((btScalar)0.0001)
52 #define GJK_DUPLICATED_EPS ((btScalar)0.0001)
53#endif //BT_USE_DOUBLE_PRECISION
54
55
56#define GJK_SIMPLEX2_EPS ((btScalar)0.0)
57#define GJK_SIMPLEX3_EPS ((btScalar)0.0)
58#define GJK_SIMPLEX4_EPS ((btScalar)0.0)
59
60 /* EPA */
61#define EPA_MAX_VERTICES 128
62#define EPA_MAX_ITERATIONS 255
63
64#ifdef BT_USE_DOUBLE_PRECISION
65 #define EPA_ACCURACY ((btScalar)1e-12)
66 #define EPA_PLANE_EPS ((btScalar)1e-14)
67 #define EPA_INSIDE_EPS ((btScalar)1e-9)
68#else
69 #define EPA_ACCURACY ((btScalar)0.0001)
70 #define EPA_PLANE_EPS ((btScalar)0.00001)
71 #define EPA_INSIDE_EPS ((btScalar)0.01)
72#endif
73
74#define EPA_FALLBACK (10*EPA_ACCURACY)
75#define EPA_MAX_FACES (EPA_MAX_VERTICES*2)
76
77
78 // Shorthands
79 typedef unsigned int U;
80 typedef unsigned char U1;
81
82 // MinkowskiDiff
84 {
88#ifdef __SPU__
89 bool m_enableMargin;
90#else
92#endif//__SPU__
93
94
96 {
97
98 }
99#ifdef __SPU__
100 void EnableMargin(bool enable)
101 {
102 m_enableMargin = enable;
103 }
104 inline btVector3 Support0(const btVector3& d) const
105 {
106 if (m_enableMargin)
107 {
109 } else
110 {
112 }
113 }
114 inline btVector3 Support1(const btVector3& d) const
115 {
116 if (m_enableMargin)
117 {
119 } else
120 {
122 }
123 }
124#else
125 void EnableMargin(bool enable)
126 {
127 if(enable)
129 else
131 }
132 inline btVector3 Support0(const btVector3& d) const
133 {
134 return(((m_shapes[0])->*(Ls))(d));
135 }
136 inline btVector3 Support1(const btVector3& d) const
137 {
138 return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d));
139 }
140#endif //__SPU__
141
142 inline btVector3 Support(const btVector3& d) const
143 {
144 return(Support0(d)-Support1(-d));
145 }
146 btVector3 Support(const btVector3& d,U index) const
147 {
148 if(index)
149 return(Support1(d));
150 else
151 return(Support0(d));
152 }
153 };
154
156
157
158 // GJK
159 struct GJK
160 {
161 /* Types */
162 struct sSV
163 {
165 };
166 struct sSimplex
167 {
168 sSV* c[4];
171 };
172 struct eStatus { enum _ {
176 /* Fields */
187 /* Methods */
189 {
190 Initialize();
191 }
193 {
194 m_ray = btVector3(0,0,0);
195 m_nfree = 0;
197 m_current = 0;
198 m_distance = 0;
199 }
200 eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess)
201 {
202 U iterations=0;
203 btScalar sqdist=0;
204 btScalar alpha=0;
205 btVector3 lastw[4];
206 U clastw=0;
207 /* Initialize solver */
208 m_free[0] = &m_store[0];
209 m_free[1] = &m_store[1];
210 m_free[2] = &m_store[2];
211 m_free[3] = &m_store[3];
212 m_nfree = 4;
213 m_current = 0;
215 m_shape = shapearg;
216 m_distance = 0;
217 /* Initialize simplex */
218 m_simplices[0].rank = 0;
219 m_ray = guess;
220 const btScalar sqrl= m_ray.length2();
221 appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0));
222 m_simplices[0].p[0] = 1;
223 m_ray = m_simplices[0].c[0]->w;
224 sqdist = sqrl;
225 lastw[0] =
226 lastw[1] =
227 lastw[2] =
228 lastw[3] = m_ray;
229 /* Loop */
230 do {
231 const U next=1-m_current;
233 sSimplex& ns=m_simplices[next];
234 /* Check zero */
235 const btScalar rl=m_ray.length();
236 if(rl<GJK_MIN_DISTANCE)
237 {/* Touching or inside */
239 break;
240 }
241 /* Append new vertice in -'v' direction */
242 appendvertice(cs,-m_ray);
243 const btVector3& w=cs.c[cs.rank-1]->w;
244 bool found=false;
245 for(U i=0;i<4;++i)
246 {
247 if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
248 { found=true;break; }
249 }
250 if(found)
251 {/* Return old simplex */
253 break;
254 }
255 else
256 {/* Update lastw */
257 lastw[clastw=(clastw+1)&3]=w;
258 }
259 /* Check for termination */
260 const btScalar omega=btDot(m_ray,w)/rl;
261 alpha=btMax(omega,alpha);
262 if(((rl-alpha)-(GJK_ACCURACY*rl))<=0)
263 {/* Return old simplex */
265 break;
266 }
267 /* Reduce simplex */
268 btScalar weights[4];
269 U mask=0;
270 switch(cs.rank)
271 {
272 case 2: sqdist=projectorigin( cs.c[0]->w,
273 cs.c[1]->w,
274 weights,mask);break;
275 case 3: sqdist=projectorigin( cs.c[0]->w,
276 cs.c[1]->w,
277 cs.c[2]->w,
278 weights,mask);break;
279 case 4: sqdist=projectorigin( cs.c[0]->w,
280 cs.c[1]->w,
281 cs.c[2]->w,
282 cs.c[3]->w,
283 weights,mask);break;
284 }
285 if(sqdist>=0)
286 {/* Valid */
287 ns.rank = 0;
288 m_ray = btVector3(0,0,0);
289 m_current = next;
290 for(U i=0,ni=cs.rank;i<ni;++i)
291 {
292 if(mask&(1<<i))
293 {
294 ns.c[ns.rank] = cs.c[i];
295 ns.p[ns.rank++] = weights[i];
296 m_ray += cs.c[i]->w*weights[i];
297 }
298 else
299 {
300 m_free[m_nfree++] = cs.c[i];
301 }
302 }
303 if(mask==15) m_status=eStatus::Inside;
304 }
305 else
306 {/* Return old simplex */
308 break;
309 }
311 } while(m_status==eStatus::Valid);
313 switch(m_status)
314 {
316 case eStatus::Inside: m_distance=0;break;
317 default:
318 {
319 }
320 }
321 return(m_status);
322 }
324 {
325 switch(m_simplex->rank)
326 {
327 case 1:
328 {
329 for(U i=0;i<3;++i)
330 {
331 btVector3 axis=btVector3(0,0,0);
332 axis[i]=1;
333 appendvertice(*m_simplex, axis);
334 if(EncloseOrigin()) return(true);
336 appendvertice(*m_simplex,-axis);
337 if(EncloseOrigin()) return(true);
339 }
340 }
341 break;
342 case 2:
343 {
344 const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w;
345 for(U i=0;i<3;++i)
346 {
347 btVector3 axis=btVector3(0,0,0);
348 axis[i]=1;
349 const btVector3 p=btCross(d,axis);
350 if(p.length2()>0)
351 {
353 if(EncloseOrigin()) return(true);
356 if(EncloseOrigin()) return(true);
358 }
359 }
360 }
361 break;
362 case 3:
363 {
364 const btVector3 n=btCross(m_simplex->c[1]->w-m_simplex->c[0]->w,
365 m_simplex->c[2]->w-m_simplex->c[0]->w);
366 if(n.length2()>0)
367 {
369 if(EncloseOrigin()) return(true);
372 if(EncloseOrigin()) return(true);
374 }
375 }
376 break;
377 case 4:
378 {
379 if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w,
380 m_simplex->c[1]->w-m_simplex->c[3]->w,
381 m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
382 return(true);
383 }
384 break;
385 }
386 return(false);
387 }
388 /* Internals */
389 void getsupport(const btVector3& d,sSV& sv) const
390 {
391 sv.d = d/d.length();
392 sv.w = m_shape.Support(sv.d);
393 }
394 void removevertice(sSimplex& simplex)
395 {
396 m_free[m_nfree++]=simplex.c[--simplex.rank];
397 }
398 void appendvertice(sSimplex& simplex,const btVector3& v)
399 {
400 simplex.p[simplex.rank]=0;
401 simplex.c[simplex.rank]=m_free[--m_nfree];
402 getsupport(v,*simplex.c[simplex.rank++]);
403 }
404 static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c)
405 {
406 return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()-
407 a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+
408 a.x()*b.y()*c.z()-a.z()*b.y()*c.x());
409 }
411 const btVector3& b,
412 btScalar* w,U& m)
413 {
414 const btVector3 d=b-a;
415 const btScalar l=d.length2();
416 if(l>GJK_SIMPLEX2_EPS)
417 {
418 const btScalar t(l>0?-btDot(a,d)/l:0);
419 if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); }
420 else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); }
421 else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
422 }
423 return(-1);
424 }
426 const btVector3& b,
427 const btVector3& c,
428 btScalar* w,U& m)
429 {
430 static const U imd3[]={1,2,0};
431 const btVector3* vt[]={&a,&b,&c};
432 const btVector3 dl[]={a-b,b-c,c-a};
433 const btVector3 n=btCross(dl[0],dl[1]);
434 const btScalar l=n.length2();
435 if(l>GJK_SIMPLEX3_EPS)
436 {
437 btScalar mindist=-1;
438 btScalar subw[2]={0.f,0.f};
439 U subm(0);
440 for(U i=0;i<3;++i)
441 {
442 if(btDot(*vt[i],btCross(dl[i],n))>0)
443 {
444 const U j=imd3[i];
445 const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm));
446 if((mindist<0)||(subd<mindist))
447 {
448 mindist = subd;
449 m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
450 w[i] = subw[0];
451 w[j] = subw[1];
452 w[imd3[j]] = 0;
453 }
454 }
455 }
456 if(mindist<0)
457 {
458 const btScalar d=btDot(a,n);
459 const btScalar s=btSqrt(l);
460 const btVector3 p=n*(d/l);
461 mindist = p.length2();
462 m = 7;
463 w[0] = (btCross(dl[1],b-p)).length()/s;
464 w[1] = (btCross(dl[2],c-p)).length()/s;
465 w[2] = 1-(w[0]+w[1]);
466 }
467 return(mindist);
468 }
469 return(-1);
470 }
472 const btVector3& b,
473 const btVector3& c,
474 const btVector3& d,
475 btScalar* w,U& m)
476 {
477 static const U imd3[]={1,2,0};
478 const btVector3* vt[]={&a,&b,&c,&d};
479 const btVector3 dl[]={a-d,b-d,c-d};
480 const btScalar vl=det(dl[0],dl[1],dl[2]);
481 const bool ng=(vl*btDot(a,btCross(b-c,a-b)))<=0;
482 if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
483 {
484 btScalar mindist=-1;
485 btScalar subw[3]={0.f,0.f,0.f};
486 U subm(0);
487 for(U i=0;i<3;++i)
488 {
489 const U j=imd3[i];
490 const btScalar s=vl*btDot(d,btCross(dl[i],dl[j]));
491 if(s>0)
492 {
493 const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
494 if((mindist<0)||(subd<mindist))
495 {
496 mindist = subd;
497 m = static_cast<U>((subm&1?1<<i:0)+
498 (subm&2?1<<j:0)+
499 (subm&4?8:0));
500 w[i] = subw[0];
501 w[j] = subw[1];
502 w[imd3[j]] = 0;
503 w[3] = subw[2];
504 }
505 }
506 }
507 if(mindist<0)
508 {
509 mindist = 0;
510 m = 15;
511 w[0] = det(c,b,d)/vl;
512 w[1] = det(a,c,d)/vl;
513 w[2] = det(b,a,d)/vl;
514 w[3] = 1-(w[0]+w[1]+w[2]);
515 }
516 return(mindist);
517 }
518 return(-1);
519 }
520 };
521
522 // EPA
523 struct EPA
524 {
525 /* Types */
526 typedef GJK::sSV sSV;
527 struct sFace
528 {
531 sSV* c[3];
532 sFace* f[3];
533 sFace* l[2];
534 U1 e[3];
536 };
537 struct sList
538 {
541 sList() : root(0),count(0) {}
542 };
543 struct sHorizon
544 {
548 sHorizon() : cf(0),ff(0),nf(0) {}
549 };
550 struct eStatus { enum _ {
561 /* Fields */
571 /* Methods */
573 {
574 Initialize();
575 }
576
577
578 static inline void bind(sFace* fa,U ea,sFace* fb,U eb)
579 {
580 fa->e[ea]=(U1)eb;fa->f[ea]=fb;
581 fb->e[eb]=(U1)ea;fb->f[eb]=fa;
582 }
583 static inline void append(sList& list,sFace* face)
584 {
585 face->l[0] = 0;
586 face->l[1] = list.root;
587 if(list.root) list.root->l[0]=face;
588 list.root = face;
589 ++list.count;
590 }
591 static inline void remove(sList& list,sFace* face)
592 {
593 if(face->l[1]) face->l[1]->l[0]=face->l[0];
594 if(face->l[0]) face->l[0]->l[1]=face->l[1];
595 if(face==list.root) list.root=face->l[1];
596 --list.count;
597 }
598
599
601 {
603 m_normal = btVector3(0,0,0);
604 m_depth = 0;
605 m_nextsv = 0;
606 for(U i=0;i<EPA_MAX_FACES;++i)
607 {
609 }
610 }
611 eStatus::_ Evaluate(GJK& gjk,const btVector3& guess)
612 {
613 GJK::sSimplex& simplex=*gjk.m_simplex;
614 if((simplex.rank>1)&&gjk.EncloseOrigin())
615 {
616
617 /* Clean up */
618 while(m_hull.root)
619 {
620 sFace* f = m_hull.root;
621 remove(m_hull,f);
622 append(m_stock,f);
623 }
625 m_nextsv = 0;
626 /* Orient simplex */
627 if(gjk.det( simplex.c[0]->w-simplex.c[3]->w,
628 simplex.c[1]->w-simplex.c[3]->w,
629 simplex.c[2]->w-simplex.c[3]->w)<0)
630 {
631 btSwap(simplex.c[0],simplex.c[1]);
632 btSwap(simplex.p[0],simplex.p[1]);
633 }
634 /* Build initial hull */
635 sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
636 newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
637 newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
638 newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
639 if(m_hull.count==4)
640 {
641 sFace* best=findbest();
642 sFace outer=*best;
643 U pass=0;
644 U iterations=0;
645 bind(tetra[0],0,tetra[1],0);
646 bind(tetra[0],1,tetra[2],0);
647 bind(tetra[0],2,tetra[3],0);
648 bind(tetra[1],1,tetra[3],2);
649 bind(tetra[1],2,tetra[2],1);
650 bind(tetra[2],2,tetra[3],1);
652 for(;iterations<EPA_MAX_ITERATIONS;++iterations)
653 {
655 {
656 sHorizon horizon;
657 sSV* w=&m_sv_store[m_nextsv++];
658 bool valid=true;
659 best->pass = (U1)(++pass);
660 gjk.getsupport(best->n,*w);
661 const btScalar wdist=btDot(best->n,w->w)-best->d;
662 if(wdist>EPA_ACCURACY)
663 {
664 for(U j=0;(j<3)&&valid;++j)
665 {
666 valid&=expand( pass,w,
667 best->f[j],best->e[j],
668 horizon);
669 }
670 if(valid&&(horizon.nf>=3))
671 {
672 bind(horizon.cf,1,horizon.ff,2);
673 remove(m_hull,best);
674 append(m_stock,best);
675 best=findbest();
676 outer=*best;
677 } else { m_status=eStatus::InvalidHull;break; }
678 } else { m_status=eStatus::AccuraryReached;break; }
679 } else { m_status=eStatus::OutOfVertices;break; }
680 }
681 const btVector3 projection=outer.n*outer.d;
682 m_normal = outer.n;
683 m_depth = outer.d;
684 m_result.rank = 3;
685 m_result.c[0] = outer.c[0];
686 m_result.c[1] = outer.c[1];
687 m_result.c[2] = outer.c[2];
688 m_result.p[0] = btCross( outer.c[1]->w-projection,
689 outer.c[2]->w-projection).length();
690 m_result.p[1] = btCross( outer.c[2]->w-projection,
691 outer.c[0]->w-projection).length();
692 m_result.p[2] = btCross( outer.c[0]->w-projection,
693 outer.c[1]->w-projection).length();
694 const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
695 m_result.p[0] /= sum;
696 m_result.p[1] /= sum;
697 m_result.p[2] /= sum;
698 return(m_status);
699 }
700 }
701 /* Fallback */
703 m_normal = -guess;
704 const btScalar nl=m_normal.length();
705 if(nl>0)
706 m_normal = m_normal/nl;
707 else
708 m_normal = btVector3(1,0,0);
709 m_depth = 0;
710 m_result.rank=1;
711 m_result.c[0]=simplex.c[0];
712 m_result.p[0]=1;
713 return(m_status);
714 }
715 bool getedgedist(sFace* face, sSV* a, sSV* b, btScalar& dist)
716 {
717 const btVector3 ba = b->w - a->w;
718 const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane
719 const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required
720
721 if(a_dot_nab < 0)
722 {
723 // Outside of edge a->b
724
725 const btScalar ba_l2 = ba.length2();
726 const btScalar a_dot_ba = btDot(a->w, ba);
727 const btScalar b_dot_ba = btDot(b->w, ba);
728
729 if(a_dot_ba > 0)
730 {
731 // Pick distance vertex a
732 dist = a->w.length();
733 }
734 else if(b_dot_ba < 0)
735 {
736 // Pick distance vertex b
737 dist = b->w.length();
738 }
739 else
740 {
741 // Pick distance to edge a->b
742 const btScalar a_dot_b = btDot(a->w, b->w);
743 dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0));
744 }
745
746 return true;
747 }
748
749 return false;
750 }
751 sFace* newface(sSV* a,sSV* b,sSV* c,bool forced)
752 {
753 if(m_stock.root)
754 {
755 sFace* face=m_stock.root;
756 remove(m_stock,face);
757 append(m_hull,face);
758 face->pass = 0;
759 face->c[0] = a;
760 face->c[1] = b;
761 face->c[2] = c;
762 face->n = btCross(b->w-a->w,c->w-a->w);
763 const btScalar l=face->n.length();
764 const bool v=l>EPA_ACCURACY;
765
766 if(v)
767 {
768 if(!(getedgedist(face, a, b, face->d) ||
769 getedgedist(face, b, c, face->d) ||
770 getedgedist(face, c, a, face->d)))
771 {
772 // Origin projects to the interior of the triangle
773 // Use distance to triangle plane
774 face->d = btDot(a->w, face->n) / l;
775 }
776
777 face->n /= l;
778 if(forced || (face->d >= -EPA_PLANE_EPS))
779 {
780 return face;
781 }
782 else
784 }
785 else
787
788 remove(m_hull, face);
789 append(m_stock, face);
790 return 0;
791
792 }
794 return 0;
795 }
797 {
798 sFace* minf=m_hull.root;
799 btScalar mind=minf->d*minf->d;
800 for(sFace* f=minf->l[1];f;f=f->l[1])
801 {
802 const btScalar sqd=f->d*f->d;
803 if(sqd<mind)
804 {
805 minf=f;
806 mind=sqd;
807 }
808 }
809 return(minf);
810 }
811 bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
812 {
813 static const U i1m3[]={1,2,0};
814 static const U i2m3[]={2,0,1};
815 if(f->pass!=pass)
816 {
817 const U e1=i1m3[e];
818 if((btDot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
819 {
820 sFace* nf=newface(f->c[e1],f->c[e],w,false);
821 if(nf)
822 {
823 bind(nf,0,f,e);
824 if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
825 horizon.cf=nf;
826 ++horizon.nf;
827 return(true);
828 }
829 }
830 else
831 {
832 const U e2=i2m3[e];
833 f->pass = (U1)pass;
834 if( expand(pass,w,f->f[e1],f->e[e1],horizon)&&
835 expand(pass,w,f->f[e2],f->e[e2],horizon))
836 {
837 remove(m_hull,f);
838 append(m_stock,f);
839 return(true);
840 }
841 }
842 }
843 return(false);
844 }
845
846 };
847
848 //
849 static void Initialize( const btConvexShape* shape0,const btTransform& wtrs0,
850 const btConvexShape* shape1,const btTransform& wtrs1,
852 tShape& shape,
853 bool withmargins)
854 {
855 /* Results */
856 results.witnesses[0] =
857 results.witnesses[1] = btVector3(0,0,0);
859 /* Shape */
860 shape.m_shapes[0] = shape0;
861 shape.m_shapes[1] = shape1;
862 shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
863 shape.m_toshape0 = wtrs0.inverseTimes(wtrs1);
864 shape.EnableMargin(withmargins);
865 }
866
867}
868
869//
870// Api
871//
872
873using namespace gjkepa2_impl;
874
875//
877{
878 return(sizeof(GJK)+sizeof(EPA));
879}
880
881//
883 const btTransform& wtrs0,
884 const btConvexShape* shape1,
885 const btTransform& wtrs1,
886 const btVector3& guess,
887 sResults& results)
888{
889 tShape shape;
890 Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
891 GJK gjk;
892 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
893 if(gjk_status==GJK::eStatus::Valid)
894 {
895 btVector3 w0=btVector3(0,0,0);
896 btVector3 w1=btVector3(0,0,0);
897 for(U i=0;i<gjk.m_simplex->rank;++i)
898 {
899 const btScalar p=gjk.m_simplex->p[i];
900 w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
901 w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
902 }
903 results.witnesses[0] = wtrs0*w0;
904 results.witnesses[1] = wtrs0*w1;
905 results.normal = w0-w1;
906 results.distance = results.normal.length();
907 results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1;
908 return(true);
909 }
910 else
911 {
912 results.status = gjk_status==GJK::eStatus::Inside?
915 return(false);
916 }
917}
918
919//
921 const btTransform& wtrs0,
922 const btConvexShape* shape1,
923 const btTransform& wtrs1,
924 const btVector3& guess,
925 sResults& results,
926 bool usemargins)
927{
928 tShape shape;
929 Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
930 GJK gjk;
931 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess);
932 switch(gjk_status)
933 {
934 case GJK::eStatus::Inside:
935 {
936 EPA epa;
937 EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess);
938 if(epa_status!=EPA::eStatus::Failed)
939 {
940 btVector3 w0=btVector3(0,0,0);
941 for(U i=0;i<epa.m_result.rank;++i)
942 {
943 w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
944 }
946 results.witnesses[0] = wtrs0*w0;
947 results.witnesses[1] = wtrs0*(w0-epa.m_normal*epa.m_depth);
948 results.normal = -epa.m_normal;
949 results.distance = -epa.m_depth;
950 return(true);
951 } else results.status=sResults::EPA_Failed;
952 }
953 break;
954 case GJK::eStatus::Failed:
956 break;
957 default:
958 {
959 }
960 }
961 return(false);
962}
963
964#ifndef __SPU__
965//
967 btScalar margin,
968 const btConvexShape* shape0,
969 const btTransform& wtrs0,
970 sResults& results)
971{
972 tShape shape;
973 btSphereShape shape1(margin);
974 btTransform wtrs1(btQuaternion(0,0,0,1),position);
975 Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
976 GJK gjk;
977 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));
978 if(gjk_status==GJK::eStatus::Valid)
979 {
980 btVector3 w0=btVector3(0,0,0);
981 btVector3 w1=btVector3(0,0,0);
982 for(U i=0;i<gjk.m_simplex->rank;++i)
983 {
984 const btScalar p=gjk.m_simplex->p[i];
985 w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
986 w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
987 }
988 results.witnesses[0] = wtrs0*w0;
989 results.witnesses[1] = wtrs0*w1;
990 const btVector3 delta= results.witnesses[1]-
991 results.witnesses[0];
992 const btScalar margin= shape0->getMarginNonVirtual()+
993 shape1.getMarginNonVirtual();
994 const btScalar length= delta.length();
995 results.normal = delta/length;
996 results.witnesses[0] += results.normal*margin;
997 return(length-margin);
998 }
999 else
1000 {
1001 if(gjk_status==GJK::eStatus::Inside)
1002 {
1003 if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results))
1004 {
1005 const btVector3 delta= results.witnesses[0]-
1006 results.witnesses[1];
1007 const btScalar length= delta.length();
1008 if (length >= SIMD_EPSILON)
1009 results.normal = delta/length;
1010 return(-length);
1011 }
1012 }
1013 }
1014 return(SIMD_INFINITY);
1015}
1016
1017//
1019 const btTransform& wtrs0,
1020 const btConvexShape* shape1,
1021 const btTransform& wtrs1,
1022 const btVector3& guess,
1023 sResults& results)
1024{
1025 if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
1026 return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false));
1027 else
1028 return(true);
1029}
1030#endif //__SPU__
1031
1032/* Symbols cleanup */
1033
1034#undef GJK_MAX_ITERATIONS
1035#undef GJK_ACCURACY
1036#undef GJK_MIN_DISTANCE
1037#undef GJK_DUPLICATED_EPS
1038#undef GJK_SIMPLEX2_EPS
1039#undef GJK_SIMPLEX3_EPS
1040#undef GJK_SIMPLEX4_EPS
1041
1042#undef EPA_MAX_VERTICES
1043#undef EPA_MAX_FACES
1044#undef EPA_MAX_ITERATIONS
1045#undef EPA_ACCURACY
1046#undef EPA_FALLBACK
1047#undef EPA_PLANE_EPS
1048#undef EPA_INSIDE_EPS
#define EPA_ACCURACY
Definition: btGjkEpa2.cpp:69
#define EPA_MAX_FACES
Definition: btGjkEpa2.cpp:75
#define GJK_MIN_DISTANCE
Definition: btGjkEpa2.cpp:51
#define GJK_SIMPLEX4_EPS
Definition: btGjkEpa2.cpp:58
#define EPA_PLANE_EPS
Definition: btGjkEpa2.cpp:70
#define GJK_DUPLICATED_EPS
Definition: btGjkEpa2.cpp:52
#define GJK_ACCURACY
Definition: btGjkEpa2.cpp:50
#define EPA_MAX_VERTICES
Definition: btGjkEpa2.cpp:61
#define GJK_SIMPLEX2_EPS
Definition: btGjkEpa2.cpp:56
#define GJK_MAX_ITERATIONS
Definition: btGjkEpa2.cpp:43
#define GJK_SIMPLEX3_EPS
Definition: btGjkEpa2.cpp:57
#define EPA_MAX_ITERATIONS
Definition: btGjkEpa2.cpp:62
static void Initialize(const btConvexTemplate &a, const btConvexTemplate &b, btGjkEpaSolver3::sResults &results, MinkowskiDiff< btConvexTemplate > &shape)
Definition: btGjkEpa3.h:842
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_INFINITY
Definition: btScalar.h:522
#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 btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:32
btScalar getMarginNonVirtual() const
btVector3 localGetSupportVertexNonVirtual(const btVector3 &vec) const
btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3 &vec) const
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 btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:55
The btSphereShape implements an implicit sphere, centered around a local origin with radius.
Definition: btSphereShape.h:24
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
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
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:849
MinkowskiDiff tShape
Definition: btGjkEpa2.cpp:155
unsigned int U
Definition: btGjkEpa2.cpp:79
unsigned char U1
Definition: btGjkEpa2.cpp:80
enum btGjkEpaSolver2::sResults::eStatus status
btVector3 witnesses[2]
Definition: btGjkEpa2.h:42
static btScalar SignedDistance(const btVector3 &position, btScalar margin, const btConvexShape *shape, const btTransform &wtrs, sResults &results)
Definition: btGjkEpa2.cpp:966
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:920
static bool Distance(const btConvexShape *shape0, const btTransform &wtrs0, const btConvexShape *shape1, const btTransform &wtrs1, const btVector3 &guess, sResults &results)
Definition: btGjkEpa2.cpp:882
static int StackSizeRequirement()
Definition: btGjkEpa2.cpp:876
bool expand(U pass, sSV *w, sFace *f, U e, sHorizon &horizon)
Definition: btGjkEpa2.cpp:811
static void remove(sList &list, sFace *face)
Definition: btGjkEpa2.cpp:591
btVector3 m_normal
Definition: btGjkEpa2.cpp:564
GJK::sSimplex m_result
Definition: btGjkEpa2.cpp:563
sFace * findbest()
Definition: btGjkEpa2.cpp:796
sSV m_sv_store[EPA_MAX_VERTICES]
Definition: btGjkEpa2.cpp:566
eStatus::_ Evaluate(GJK &gjk, const btVector3 &guess)
Definition: btGjkEpa2.cpp:611
sFace m_fc_store[EPA_MAX_FACES]
Definition: btGjkEpa2.cpp:567
static void append(sList &list, sFace *face)
Definition: btGjkEpa2.cpp:583
sFace * newface(sSV *a, sSV *b, sSV *c, bool forced)
Definition: btGjkEpa2.cpp:751
eStatus::_ m_status
Definition: btGjkEpa2.cpp:562
static void bind(sFace *fa, U ea, sFace *fb, U eb)
Definition: btGjkEpa2.cpp:578
bool getedgedist(sFace *face, sSV *a, sSV *b, btScalar &dist)
Definition: btGjkEpa2.cpp:715
sSimplex m_simplices[2]
Definition: btGjkEpa2.cpp:180
sSimplex * m_simplex
Definition: btGjkEpa2.cpp:185
void removevertice(sSimplex &simplex)
Definition: btGjkEpa2.cpp:394
void getsupport(const btVector3 &d, sSV &sv) const
Definition: btGjkEpa2.cpp:389
btScalar m_distance
Definition: btGjkEpa2.cpp:179
eStatus::_ m_status
Definition: btGjkEpa2.cpp:186
static btScalar det(const btVector3 &a, const btVector3 &b, const btVector3 &c)
Definition: btGjkEpa2.cpp:404
void appendvertice(sSimplex &simplex, const btVector3 &v)
Definition: btGjkEpa2.cpp:398
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, btScalar *w, U &m)
Definition: btGjkEpa2.cpp:425
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &d, btScalar *w, U &m)
Definition: btGjkEpa2.cpp:471
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, btScalar *w, U &m)
Definition: btGjkEpa2.cpp:410
eStatus::_ Evaluate(const tShape &shapearg, const btVector3 &guess)
Definition: btGjkEpa2.cpp:200
btVector3 Support0(const btVector3 &d) const
Definition: btGjkEpa2.cpp:132
void EnableMargin(bool enable)
Definition: btGjkEpa2.cpp:125
const btConvexShape * m_shapes[2]
Definition: btGjkEpa2.cpp:85
btVector3 Support1(const btVector3 &d) const
Definition: btGjkEpa2.cpp:136
btVector3 Support(const btVector3 &d, U index) const
Definition: btGjkEpa2.cpp:146
btVector3(btConvexShape::* Ls)(const btVector3 &) const
Definition: btGjkEpa2.cpp:91
btVector3 Support(const btVector3 &d) const
Definition: btGjkEpa2.cpp:142