2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
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
8 Permission is granted to anyone to use this software for any purpose,
9 including commercial applications, and to alter it and redistribute it
11 subject to the following restrictions:
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
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.
23 GJK-EPA collision solver by Nathanael Presson, 2008
25 #include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
26 #include "BulletCollision/CollisionShapes/btSphereShape.h"
27 #include "btGjkEpa2.h"
29 #if defined(DEBUG) || defined (_DEBUG)
30 #include <stdio.h> //for debug printf
32 #include <spu_printf.h>
33 #define printf spu_printf
37 namespace gjkepa2_impl
43 #define GJK_MAX_ITERATIONS 128
44 #define GJK_ACCURARY ((btScalar)0.0001)
45 #define GJK_MIN_DISTANCE ((btScalar)0.0001)
46 #define GJK_DUPLICATED_EPS ((btScalar)0.0001)
47 #define GJK_SIMPLEX2_EPS ((btScalar)0.0)
48 #define GJK_SIMPLEX3_EPS ((btScalar)0.0)
49 #define GJK_SIMPLEX4_EPS ((btScalar)0.0)
52 #define EPA_MAX_VERTICES 64
53 #define EPA_MAX_FACES (EPA_MAX_VERTICES*2)
54 #define EPA_MAX_ITERATIONS 255
55 #define EPA_ACCURACY ((btScalar)0.0001)
56 #define EPA_FALLBACK (10*EPA_ACCURACY)
57 #define EPA_PLANE_EPS ((btScalar)0.00001)
58 #define EPA_INSIDE_EPS ((btScalar)0.01)
62 typedef unsigned int U;
63 typedef unsigned char U1;
68 const btConvexShape* m_shapes[2];
69 btMatrix3x3 m_toshape1;
70 btTransform m_toshape0;
74 btVector3 (btConvexShape::*Ls)(const btVector3&) const;
83 void EnableMargin(bool enable)
85 m_enableMargin = enable;
87 inline btVector3 Support0(const btVector3& d) const
91 return m_shapes[0]->localGetSupportVertexNonVirtual(d);
94 return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d);
97 inline btVector3 Support1(const btVector3& d) const
101 return m_toshape0*(m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1*d));
104 return m_toshape0*(m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1*d));
108 void EnableMargin(bool enable)
111 Ls=&btConvexShape::localGetSupportVertexNonVirtual;
113 Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual;
115 inline btVector3 Support0(const btVector3& d) const
117 return(((m_shapes[0])->*(Ls))(d));
119 inline btVector3 Support1(const btVector3& d) const
121 return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d));
125 inline btVector3 Support(const btVector3& d) const
127 return(Support0(d)-Support1(-d));
129 btVector3 Support(const btVector3& d,U index) const
138 typedef MinkowskiDiff tShape;
155 struct eStatus { enum _ {
163 sSimplex m_simplices[2];
177 m_ray = btVector3(0,0,0);
179 m_status = eStatus::Failed;
183 eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess)
190 /* Initialize solver */
191 m_free[0] = &m_store[0];
192 m_free[1] = &m_store[1];
193 m_free[2] = &m_store[2];
194 m_free[3] = &m_store[3];
197 m_status = eStatus::Valid;
200 /* Initialize simplex */
201 m_simplices[0].rank = 0;
203 const btScalar sqrl= m_ray.length2();
204 appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0));
205 m_simplices[0].p[0] = 1;
206 m_ray = m_simplices[0].c[0]->w;
214 const U next=1-m_current;
215 sSimplex& cs=m_simplices[m_current];
216 sSimplex& ns=m_simplices[next];
218 const btScalar rl=m_ray.length();
219 if(rl<GJK_MIN_DISTANCE)
220 {/* Touching or inside */
221 m_status=eStatus::Inside;
224 /* Append new vertice in -'v' direction */
225 appendvertice(cs,-m_ray);
226 const btVector3& w=cs.c[cs.rank-1]->w;
230 if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
231 { found=true;break; }
234 {/* Return old simplex */
235 removevertice(m_simplices[m_current]);
240 lastw[clastw=(clastw+1)&3]=w;
242 /* Check for termination */
243 const btScalar omega=btDot(m_ray,w)/rl;
244 alpha=btMax(omega,alpha);
245 if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
246 {/* Return old simplex */
247 removevertice(m_simplices[m_current]);
255 case 2: sqdist=projectorigin( cs.c[0]->w,
258 case 3: sqdist=projectorigin( cs.c[0]->w,
262 case 4: sqdist=projectorigin( cs.c[0]->w,
271 m_ray = btVector3(0,0,0);
273 for(U i=0,ni=cs.rank;i<ni;++i)
277 ns.c[ns.rank] = cs.c[i];
278 ns.p[ns.rank++] = weights[i];
279 m_ray += cs.c[i]->w*weights[i];
283 m_free[m_nfree++] = cs.c[i];
286 if(mask==15) m_status=eStatus::Inside;
289 {/* Return old simplex */
290 removevertice(m_simplices[m_current]);
293 m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
294 } while(m_status==eStatus::Valid);
295 m_simplex=&m_simplices[m_current];
298 case eStatus::Valid: m_distance=m_ray.length();break;
299 case eStatus::Inside: m_distance=0;break;
308 switch(m_simplex->rank)
314 btVector3 axis=btVector3(0,0,0);
316 appendvertice(*m_simplex, axis);
317 if(EncloseOrigin()) return(true);
318 removevertice(*m_simplex);
319 appendvertice(*m_simplex,-axis);
320 if(EncloseOrigin()) return(true);
321 removevertice(*m_simplex);
327 const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w;
330 btVector3 axis=btVector3(0,0,0);
332 const btVector3 p=btCross(d,axis);
335 appendvertice(*m_simplex, p);
336 if(EncloseOrigin()) return(true);
337 removevertice(*m_simplex);
338 appendvertice(*m_simplex,-p);
339 if(EncloseOrigin()) return(true);
340 removevertice(*m_simplex);
347 const btVector3 n=btCross(m_simplex->c[1]->w-m_simplex->c[0]->w,
348 m_simplex->c[2]->w-m_simplex->c[0]->w);
351 appendvertice(*m_simplex,n);
352 if(EncloseOrigin()) return(true);
353 removevertice(*m_simplex);
354 appendvertice(*m_simplex,-n);
355 if(EncloseOrigin()) return(true);
356 removevertice(*m_simplex);
362 if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w,
363 m_simplex->c[1]->w-m_simplex->c[3]->w,
364 m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
372 void getsupport(const btVector3& d,sSV& sv) const
375 sv.w = m_shape.Support(sv.d);
377 void removevertice(sSimplex& simplex)
379 m_free[m_nfree++]=simplex.c[--simplex.rank];
381 void appendvertice(sSimplex& simplex,const btVector3& v)
383 simplex.p[simplex.rank]=0;
384 simplex.c[simplex.rank]=m_free[--m_nfree];
385 getsupport(v,*simplex.c[simplex.rank++]);
387 static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c)
389 return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()-
390 a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+
391 a.x()*b.y()*c.z()-a.z()*b.y()*c.x());
393 static btScalar projectorigin( const btVector3& a,
397 const btVector3 d=b-a;
398 const btScalar l=d.length2();
399 if(l>GJK_SIMPLEX2_EPS)
401 const btScalar t(l>0?-btDot(a,d)/l:0);
402 if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); }
403 else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); }
404 else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
408 static btScalar projectorigin( const btVector3& a,
413 static const U imd3[]={1,2,0};
414 const btVector3* vt[]={&a,&b,&c};
415 const btVector3 dl[]={a-b,b-c,c-a};
416 const btVector3 n=btCross(dl[0],dl[1]);
417 const btScalar l=n.length2();
418 if(l>GJK_SIMPLEX3_EPS)
421 btScalar subw[2]={0.f,0.f};
425 if(btDot(*vt[i],btCross(dl[i],n))>0)
428 const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm));
429 if((mindist<0)||(subd<mindist))
432 m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
441 const btScalar d=btDot(a,n);
442 const btScalar s=btSqrt(l);
443 const btVector3 p=n*(d/l);
444 mindist = p.length2();
446 w[0] = (btCross(dl[1],b-p)).length()/s;
447 w[1] = (btCross(dl[2],c-p)).length()/s;
448 w[2] = 1-(w[0]+w[1]);
454 static btScalar projectorigin( const btVector3& a,
460 static const U imd3[]={1,2,0};
461 const btVector3* vt[]={&a,&b,&c,&d};
462 const btVector3 dl[]={a-d,b-d,c-d};
463 const btScalar vl=det(dl[0],dl[1],dl[2]);
464 const bool ng=(vl*btDot(a,btCross(b-c,a-b)))<=0;
465 if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
468 btScalar subw[3]={0.f,0.f,0.f};
473 const btScalar s=vl*btDot(d,btCross(dl[i],dl[j]));
476 const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
477 if((mindist<0)||(subd<mindist))
480 m = static_cast<U>((subm&1?1<<i:0)+
494 w[0] = det(c,b,d)/vl;
495 w[1] = det(a,c,d)/vl;
496 w[2] = det(b,a,d)/vl;
497 w[3] = 1-(w[0]+w[1]+w[2]);
509 typedef GJK::sSV sSV;
525 sList() : root(0),count(0) {}
532 sHorizon() : cf(0),ff(0),nf(0) {}
534 struct eStatus { enum _ {
547 GJK::sSimplex m_result;
550 sSV m_sv_store[EPA_MAX_VERTICES];
551 sFace m_fc_store[EPA_MAX_FACES];
562 static inline void bind(sFace* fa,U ea,sFace* fb,U eb)
564 fa->e[ea]=(U1)eb;fa->f[ea]=fb;
565 fb->e[eb]=(U1)ea;fb->f[eb]=fa;
567 static inline void append(sList& list,sFace* face)
570 face->l[1] = list.root;
571 if(list.root) list.root->l[0]=face;
575 static inline void remove(sList& list,sFace* face)
577 if(face->l[1]) face->l[1]->l[0]=face->l[0];
578 if(face->l[0]) face->l[0]->l[1]=face->l[1];
579 if(face==list.root) list.root=face->l[1];
586 m_status = eStatus::Failed;
587 m_normal = btVector3(0,0,0);
590 for(U i=0;i<EPA_MAX_FACES;++i)
592 append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
595 eStatus::_ Evaluate(GJK& gjk,const btVector3& guess)
597 GJK::sSimplex& simplex=*gjk.m_simplex;
598 if((simplex.rank>1)&&gjk.EncloseOrigin())
604 sFace* f = m_hull.root;
608 m_status = eStatus::Valid;
611 if(gjk.det( simplex.c[0]->w-simplex.c[3]->w,
612 simplex.c[1]->w-simplex.c[3]->w,
613 simplex.c[2]->w-simplex.c[3]->w)<0)
615 btSwap(simplex.c[0],simplex.c[1]);
616 btSwap(simplex.p[0],simplex.p[1]);
618 /* Build initial hull */
619 sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
620 newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
621 newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
622 newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
625 sFace* best=findbest();
629 bind(tetra[0],0,tetra[1],0);
630 bind(tetra[0],1,tetra[2],0);
631 bind(tetra[0],2,tetra[3],0);
632 bind(tetra[1],1,tetra[3],2);
633 bind(tetra[1],2,tetra[2],1);
634 bind(tetra[2],2,tetra[3],1);
635 m_status=eStatus::Valid;
636 for(;iterations<EPA_MAX_ITERATIONS;++iterations)
638 if(m_nextsv<EPA_MAX_VERTICES)
641 sSV* w=&m_sv_store[m_nextsv++];
643 best->pass = (U1)(++pass);
644 gjk.getsupport(best->n,*w);
645 const btScalar wdist=btDot(best->n,w->w)-best->d;
646 if(wdist>EPA_ACCURACY)
648 for(U j=0;(j<3)&&valid;++j)
650 valid&=expand( pass,w,
651 best->f[j],best->e[j],
654 if(valid&&(horizon.nf>=3))
656 bind(horizon.cf,1,horizon.ff,2);
658 append(m_stock,best);
660 if(best->p>=outer.p) outer=*best;
661 } else { m_status=eStatus::InvalidHull;break; }
662 } else { m_status=eStatus::AccuraryReached;break; }
663 } else { m_status=eStatus::OutOfVertices;break; }
665 const btVector3 projection=outer.n*outer.d;
669 m_result.c[0] = outer.c[0];
670 m_result.c[1] = outer.c[1];
671 m_result.c[2] = outer.c[2];
672 m_result.p[0] = btCross( outer.c[1]->w-projection,
673 outer.c[2]->w-projection).length();
674 m_result.p[1] = btCross( outer.c[2]->w-projection,
675 outer.c[0]->w-projection).length();
676 m_result.p[2] = btCross( outer.c[0]->w-projection,
677 outer.c[1]->w-projection).length();
678 const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
679 m_result.p[0] /= sum;
680 m_result.p[1] /= sum;
681 m_result.p[2] /= sum;
686 m_status = eStatus::FallBack;
688 const btScalar nl=m_normal.length();
690 m_normal = m_normal/nl;
692 m_normal = btVector3(1,0,0);
695 m_result.c[0]=simplex.c[0];
699 sFace* newface(sSV* a,sSV* b,sSV* c,bool forced)
703 sFace* face=m_stock.root;
704 remove(m_stock,face);
710 face->n = btCross(b->w-a->w,c->w-a->w);
711 const btScalar l=face->n.length();
712 const bool v=l>EPA_ACCURACY;
713 face->p = btMin(btMin(
714 btDot(a->w,btCross(face->n,a->w-b->w)),
715 btDot(b->w,btCross(face->n,b->w-c->w))),
716 btDot(c->w,btCross(face->n,c->w-a->w))) /
718 face->p = face->p>=-EPA_INSIDE_EPS?0:face->p;
721 face->d = btDot(a->w,face->n)/l;
723 if(forced||(face->d>=-EPA_PLANE_EPS))
726 } else m_status=eStatus::NonConvex;
727 } else m_status=eStatus::Degenerated;
729 append(m_stock,face);
732 m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces;
737 sFace* minf=m_hull.root;
738 btScalar mind=minf->d*minf->d;
739 btScalar maxp=minf->p;
740 for(sFace* f=minf->l[1];f;f=f->l[1])
742 const btScalar sqd=f->d*f->d;
743 if((f->p>=maxp)&&(sqd<mind))
752 bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
754 static const U i1m3[]={1,2,0};
755 static const U i2m3[]={2,0,1};
759 if((btDot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
761 sFace* nf=newface(f->c[e1],f->c[e],w,false);
765 if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
775 if( expand(pass,w,f->f[e1],f->e[e1],horizon)&&
776 expand(pass,w,f->f[e2],f->e[e2],horizon))
790 static void Initialize( const btConvexShape* shape0,const btTransform& wtrs0,
791 const btConvexShape* shape1,const btTransform& wtrs1,
792 btGjkEpaSolver2::sResults& results,
797 results.witnesses[0] =
798 results.witnesses[1] = btVector3(0,0,0);
799 results.status = btGjkEpaSolver2::sResults::Separated;
801 shape.m_shapes[0] = shape0;
802 shape.m_shapes[1] = shape1;
803 shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
804 shape.m_toshape0 = wtrs0.inverseTimes(wtrs1);
805 shape.EnableMargin(withmargins);
814 using namespace gjkepa2_impl;
817 int btGjkEpaSolver2::StackSizeRequirement()
819 return(sizeof(GJK)+sizeof(EPA));
823 bool btGjkEpaSolver2::Distance( const btConvexShape* shape0,
824 const btTransform& wtrs0,
825 const btConvexShape* shape1,
826 const btTransform& wtrs1,
827 const btVector3& guess,
831 Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
833 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
834 if(gjk_status==GJK::eStatus::Valid)
836 btVector3 w0=btVector3(0,0,0);
837 btVector3 w1=btVector3(0,0,0);
838 for(U i=0;i<gjk.m_simplex->rank;++i)
840 const btScalar p=gjk.m_simplex->p[i];
841 w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
842 w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
844 results.witnesses[0] = wtrs0*w0;
845 results.witnesses[1] = wtrs0*w1;
846 results.normal = w0-w1;
847 results.distance = results.normal.length();
848 results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1;
853 results.status = gjk_status==GJK::eStatus::Inside?
854 sResults::Penetrating :
855 sResults::GJK_Failed ;
861 bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0,
862 const btTransform& wtrs0,
863 const btConvexShape* shape1,
864 const btTransform& wtrs1,
865 const btVector3& guess,
870 Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
872 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess);
875 case GJK::eStatus::Inside:
878 EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess);
879 if(epa_status!=EPA::eStatus::Failed)
881 btVector3 w0=btVector3(0,0,0);
882 for(U i=0;i<epa.m_result.rank;++i)
884 w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
886 results.status = sResults::Penetrating;
887 results.witnesses[0] = wtrs0*w0;
888 results.witnesses[1] = wtrs0*(w0-epa.m_normal*epa.m_depth);
889 results.normal = -epa.m_normal;
890 results.distance = -epa.m_depth;
892 } else results.status=sResults::EPA_Failed;
895 case GJK::eStatus::Failed:
896 results.status=sResults::GJK_Failed;
907 btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position,
909 const btConvexShape* shape0,
910 const btTransform& wtrs0,
914 btSphereShape shape1(margin);
915 btTransform wtrs1(btQuaternion(0,0,0,1),position);
916 Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
918 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));
919 if(gjk_status==GJK::eStatus::Valid)
921 btVector3 w0=btVector3(0,0,0);
922 btVector3 w1=btVector3(0,0,0);
923 for(U i=0;i<gjk.m_simplex->rank;++i)
925 const btScalar p=gjk.m_simplex->p[i];
926 w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
927 w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
929 results.witnesses[0] = wtrs0*w0;
930 results.witnesses[1] = wtrs0*w1;
931 const btVector3 delta= results.witnesses[1]-
932 results.witnesses[0];
933 const btScalar margin= shape0->getMarginNonVirtual()+
934 shape1.getMarginNonVirtual();
935 const btScalar length= delta.length();
936 results.normal = delta/length;
937 results.witnesses[0] += results.normal*margin;
938 return(length-margin);
942 if(gjk_status==GJK::eStatus::Inside)
944 if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results))
946 const btVector3 delta= results.witnesses[0]-
947 results.witnesses[1];
948 const btScalar length= delta.length();
949 if (length >= SIMD_EPSILON)
950 results.normal = delta/length;
955 return(SIMD_INFINITY);
959 bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0,
960 const btTransform& wtrs0,
961 const btConvexShape* shape1,
962 const btTransform& wtrs1,
963 const btVector3& guess,
966 if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
967 return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false));
973 /* Symbols cleanup */
975 #undef GJK_MAX_ITERATIONS
977 #undef GJK_MIN_DISTANCE
978 #undef GJK_DUPLICATED_EPS
979 #undef GJK_SIMPLEX2_EPS
980 #undef GJK_SIMPLEX3_EPS
981 #undef GJK_SIMPLEX4_EPS
983 #undef EPA_MAX_VERTICES
985 #undef EPA_MAX_ITERATIONS
989 #undef EPA_INSIDE_EPS