Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp @ 1963

Last change on this file since 1963 was 1963, checked in by rgrieder, 16 years ago

Added Bullet physics engine.
  • Property svn:eol-style set to native
File size: 14.5 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 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
24Nov.2006
25*/
26
27#include "btGjkEpa.h"
28#include <string.h> //for memset
29#include "LinearMath/btStackAlloc.h"
30
31#if defined(DEBUG) || defined (_DEBUG)
32#include <stdio.h> //for debug printf
33#ifdef __SPU__
34#include <spu_printf.h>
35#define printf spu_printf
36#endif //__SPU__
37#endif
38
39namespace gjkepa_impl
40{
41
42//
43// Port. typedefs
44//
45
46typedef btScalar                F;
47typedef bool                    Z;
48typedef int                             I;
49typedef unsigned int    U;
50typedef unsigned char   U1;
51typedef unsigned short  U2;
52
53typedef btVector3               Vector3;
54typedef btMatrix3x3             Rotation;
55
56//
57// Config
58//
59
60#if 0
61#define BTLOCALSUPPORT  localGetSupportingVertexWithoutMargin
62#else
63#define BTLOCALSUPPORT  localGetSupportingVertex
64#endif
65
66//
67// Const
68//
69
70
71#define cstInf                          SIMD_INFINITY
72#define cstPi                           SIMD_PI
73#define cst2Pi                          SIMD_2_PI
74#define GJK_maxiterations       (128)
75#define GJK_hashsize            (1<<6)
76#define GJK_hashmask            (GJK_hashsize-1)
77#define GJK_insimplex_eps       F(0.0001)
78#define GJK_sqinsimplex_eps     (GJK_insimplex_eps*GJK_insimplex_eps)
79#define EPA_maxiterations       256
80#define EPA_inface_eps          F(0.01)
81#define EPA_accuracy            F(0.001)
82
83//
84// Utils
85//
86
87static inline F                                                         Abs(F v)                                        { return(v<0?-v:v); }
88static inline F                                                         Sign(F v)                                       { return(F(v<0?-1:1)); }
89template <typename T> static inline void        Swap(T& a,T& b)                         { T
90t(a);a=b;b=t; }
91template <typename T> static inline T           Min(const T& a,const T& b)      { 
92return(a<b?a:b); }
93template <typename T> static inline T           Max(const T& a,const T& b)      { 
94return(a>b?a:b); }
95static inline void                                                      ClearMemory(void* p,U sz)       { memset(p,0,(size_t)sz); 
96}
97#if 0
98template <typename T> static inline void        Raise(const T& object)          {
99throw(object); }
100#else
101template <typename T> static inline void        Raise(const T&)                         {}
102#endif
103
104
105
106//
107// GJK
108//
109struct  GJK
110        {
111        struct Mkv
112                {
113                Vector3 w;              /* Minkowski vertice    */
114                Vector3 r;              /* Ray                                  */
115                };
116        struct He
117                {
118                Vector3 v;
119                He*             n;
120                };
121        btStackAlloc*                           sa;
122        btBlock*                sablock;
123        He*                                             table[GJK_hashsize];
124        Rotation                                wrotations[2];
125        Vector3                                 positions[2];
126        const btConvexShape*    shapes[2];
127        Mkv                                             simplex[5];
128        Vector3                                 ray;
129        U                                               order;
130        U                                               iterations;
131        F                                               margin;
132        Z                                               failed;
133        //
134                                        GJK(btStackAlloc* psa,
135                                                const Rotation& wrot0,const Vector3& pos0,const btConvexShape* shape0,
136                                                const Rotation& wrot1,const Vector3& pos1,const btConvexShape* shape1,
137                                                F pmargin=0)
138                {
139                wrotations[0]=wrot0;positions[0]=pos0;shapes[0]=shape0;
140                wrotations[1]=wrot1;positions[1]=pos1;shapes[1]=shape1;
141                sa              =psa;
142                sablock =sa->beginBlock();
143                margin  =pmargin;
144                failed  =false;
145                }
146        //
147                                        ~GJK()
148                {
149                sa->endBlock(sablock);
150                }
151        // vdh : very dumm hash
152        static inline U Hash(const Vector3& v)
153                {
154                //this doesn't compile under GCC 3.3.5, so add the ()...
155                //const U       h(U(v[0]*15461)^U(v[1]*83003)^U(v[2]*15473));
156                //return(((*((const U*)&h))*169639)&GJK_hashmask);
157            const U h((U)(v[0]*15461)^(U)(v[1]*83003)^(U)(v[2]*15473));
158            return(((*((const U*)&h))*169639)&GJK_hashmask);
159                }
160        //
161        inline Vector3  LocalSupport(const Vector3& d,U i) const
162                {
163                return(wrotations[i]*shapes[i]->BTLOCALSUPPORT(d*wrotations[i])+positions[i]);
164                }
165        //
166        inline void             Support(const Vector3& d,Mkv& v) const
167                {
168                v.r     =       d;
169                v.w     =       LocalSupport(d,0)-LocalSupport(-d,1)+d*margin;
170                }
171        #define SPX(_i_)        simplex[_i_]
172        #define SPXW(_i_)       simplex[_i_].w
173        //
174        inline Z                FetchSupport()
175                {
176                const U                 h(Hash(ray));
177                He*                             e = (He*)(table[h]);
178                while(e) { if(e->v==ray) { --order;return(false); } else e=e->n; }
179                e=(He*)sa->allocate(sizeof(He));e->v=ray;e->n=table[h];table[h]=e;
180                Support(ray,simplex[++order]);
181                return(ray.dot(SPXW(order))>0);
182                }
183        //
184        inline Z                SolveSimplex2(const Vector3& ao,const Vector3& ab)
185                {
186                if(ab.dot(ao)>=0)
187                        {
188                        const Vector3   cabo(cross(ab,ao));
189                        if(cabo.length2()>GJK_sqinsimplex_eps)
190                                { ray=cross(cabo,ab); }
191                                else
192                                { return(true); }
193                        }
194                        else
195                        { order=0;SPX(0)=SPX(1);ray=ao; }
196                return(false);
197                }
198        //
199        inline Z                SolveSimplex3(const Vector3& ao,const Vector3& ab,const Vector3& 
200ac)
201                {
202                return(SolveSimplex3a(ao,ab,ac,cross(ab,ac)));
203                }
204        //
205        inline Z                SolveSimplex3a(const Vector3& ao,const Vector3& ab,const Vector3& 
206ac,const Vector3& cabc)
207                {
208                                if((cross(cabc,ab)).dot(ao)<-GJK_insimplex_eps)
209                        { order=1;SPX(0)=SPX(1);SPX(1)=SPX(2);return(SolveSimplex2(ao,ab));     }
210                else    if((cross(cabc,ac)).dot(ao)>+GJK_insimplex_eps)
211                        { order=1;SPX(1)=SPX(2);return(SolveSimplex2(ao,ac)); }
212                else
213                        {
214                        const F                 d(cabc.dot(ao));
215                        if(Abs(d)>GJK_insimplex_eps)
216                                {
217                                if(d>0)
218                                        { ray=cabc; }
219                                        else
220                                        { ray=-cabc;Swap(SPX(0),SPX(1)); }
221                                return(false);
222                                } else return(true);
223                        }
224                }
225        //
226        inline Z                SolveSimplex4(const Vector3& ao,const Vector3& ab,const Vector3& 
227ac,const Vector3& ad)
228                {
229                Vector3                 crs;
230                                if((crs=cross(ab,ac)).dot(ao)>GJK_insimplex_eps)
231                        { 
232order=2;SPX(0)=SPX(1);SPX(1)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ab,ac,crs)); 
233}
234                else    if((crs=cross(ac,ad)).dot(ao)>GJK_insimplex_eps)
235                        { order=2;SPX(2)=SPX(3);return(SolveSimplex3a(ao,ac,ad,crs)); }
236                else    if((crs=cross(ad,ab)).dot(ao)>GJK_insimplex_eps)
237                        { 
238order=2;SPX(1)=SPX(0);SPX(0)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ad,ab,crs)); 
239}
240                else    return(true);
241                }
242        //
243        inline Z                SearchOrigin(const Vector3& initray=Vector3(1,0,0))
244                {
245                iterations      =       0;
246                order           =       (U)-1;
247                failed          =       false;
248                ray                     =       initray.normalized();
249                ClearMemory(table,sizeof(void*)*GJK_hashsize);
250                FetchSupport();
251                ray                     =       -SPXW(0);
252                for(;iterations<GJK_maxiterations;++iterations)
253                        {
254                        const F         rl(ray.length());
255                        ray/=rl>0?rl:1;
256                        if(FetchSupport())
257                                {
258                                Z       found(false);
259                                switch(order)
260                                        {
261                                        case    1:      found=SolveSimplex2(-SPXW(1),SPXW(0)-SPXW(1));break;
262                                        case    2:      found=SolveSimplex3(-SPXW(2),SPXW(1)-SPXW(2),SPXW(0)-SPXW(2));break;
263                                        case    3:      found=SolveSimplex4(-SPXW(3),SPXW(2)-SPXW(3),SPXW(1)-SPXW(3),SPXW(0)-SPXW(3));break;
264                                        }
265                                if(found) return(true);
266                                } else return(false);
267                        }
268                failed=true;
269                return(false);
270                }
271        //
272        inline Z                EncloseOrigin()
273                {
274                switch(order)
275                        {
276                        /* Point                */
277                        case    0:      break;
278                        /* Line                 */
279                        case    1:
280                                {
281                                const Vector3   ab(SPXW(1)-SPXW(0));
282                                const Vector3   b[]={   cross(ab,Vector3(1,0,0)),
283                                                                                cross(ab,Vector3(0,1,0)),
284                                                                                cross(ab,Vector3(0,0,1))};
285                                const F                 m[]={b[0].length2(),b[1].length2(),b[2].length2()};
286                                const Rotation  r(btQuaternion(ab.normalized(),cst2Pi/3));
287                                Vector3                 w(b[m[0]>m[1]?m[0]>m[2]?0:2:m[1]>m[2]?1:2]);
288                                Support(w.normalized(),simplex[4]);w=r*w;
289                                Support(w.normalized(),simplex[2]);w=r*w;
290                                Support(w.normalized(),simplex[3]);w=r*w;
291                                order=4;
292                                return(true);
293                                }
294                        break;
295                        /* Triangle             */
296                        case    2:
297                                {
298                                const 
299Vector3 n(cross((SPXW(1)-SPXW(0)),(SPXW(2)-SPXW(0))).normalized());
300                                Support( n,simplex[3]);
301                                Support(-n,simplex[4]);
302                                order=4;
303                                return(true);
304                                }
305                        break;
306                        /* Tetrahedron  */
307                        case    3:      return(true);
308                        /* Hexahedron   */
309                        case    4:      return(true);
310                        }
311                return(false);
312                }
313        #undef SPX
314        #undef SPXW
315        };
316
317//
318// EPA
319//
320struct  EPA
321        {
322        //
323        struct Face
324                {
325                const GJK::Mkv* v[3];
326                Face*                   f[3];
327                U                               e[3];
328                Vector3                 n;
329                F                               d;
330                U                               mark;
331                Face*                   prev;
332                Face*                   next;
333                Face()                  {}
334                };
335        //
336        GJK*                    gjk;
337        btStackAlloc*           sa;
338        Face*                   root;
339        U                               nfaces;
340        U                               iterations;
341        Vector3                 features[2][3];
342        Vector3                 nearest[2];
343        Vector3                 normal;
344        F                               depth;
345        Z                               failed;
346        //
347                                        EPA(GJK* pgjk)
348                {
349                gjk             =       pgjk;
350                sa              =       pgjk->sa;
351                }
352        //
353                                        ~EPA()
354                {
355                }
356        //
357        inline Vector3  GetCoordinates(const Face* face) const
358                {
359                const Vector3   o(face->n*-face->d);
360                const F                 a[]={   cross(face->v[0]->w-o,face->v[1]->w-o).length(),
361                                                                cross(face->v[1]->w-o,face->v[2]->w-o).length(),
362                                                                cross(face->v[2]->w-o,face->v[0]->w-o).length()};
363                const F                 sm(a[0]+a[1]+a[2]);
364                return(Vector3(a[1],a[2],a[0])/(sm>0?sm:1));
365                }
366        //
367        inline Face*    FindBest() const
368                {
369                Face*   bf = 0;
370                if(root)
371                        {
372                        Face*   cf = root;
373                        F               bd(cstInf);
374                        do      {
375                                if(cf->d<bd) { bd=cf->d;bf=cf; }
376                                } while(0!=(cf=cf->next));
377                        }
378                return(bf);
379                }
380        //
381        inline Z                Set(Face* f,const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv*
382c) const
383                {
384                const Vector3   nrm(cross(b->w-a->w,c->w-a->w));
385                const F                 len(nrm.length());
386                const Z                 valid(  (cross(a->w,b->w).dot(nrm)>=-EPA_inface_eps)&&
387                                                                (cross(b->w,c->w).dot(nrm)>=-EPA_inface_eps)&&
388                                                                (cross(c->w,a->w).dot(nrm)>=-EPA_inface_eps));
389                f->v[0] =       a;
390                f->v[1] =       b;
391                f->v[2] =       c;
392                f->mark =       0;
393                f->n    =       nrm/(len>0?len:cstInf);
394                f->d    =       Max<F>(0,-f->n.dot(a->w));
395                return(valid);
396                }
397        //
398        inline Face*    NewFace(const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv* c)
399                {
400                Face*   pf = (Face*)sa->allocate(sizeof(Face));
401                if(Set(pf,a,b,c))
402                        {
403                        if(root) root->prev=pf;
404                        pf->prev=0;
405                        pf->next=root;
406                        root    =pf;
407                        ++nfaces;
408                        }
409                        else
410                        {
411                        pf->prev=pf->next=0;
412                        }
413                return(pf);
414                }
415        //
416        inline void             Detach(Face* face)
417                {
418                if(face->prev||face->next)
419                        {
420                        --nfaces;
421                        if(face==root)
422                                { root=face->next;root->prev=0; }
423                                else
424                                {
425                                if(face->next==0)
426                                        { face->prev->next=0; }
427                                        else
428                                        { face->prev->next=face->next;face->next->prev=face->prev; }
429                                }
430                        face->prev=face->next=0;
431                        }
432                }
433        //
434        inline void             Link(Face* f0,U e0,Face* f1,U e1) const
435                {
436                f0->f[e0]=f1;f1->e[e1]=e0;
437                f1->f[e1]=f0;f0->e[e0]=e1;
438                }
439        //
440        GJK::Mkv*               Support(const Vector3& w) const
441                {
442                GJK::Mkv*               v =(GJK::Mkv*)sa->allocate(sizeof(GJK::Mkv));
443                gjk->Support(w,*v);
444                return(v);
445                }
446        //
447        U                               BuildHorizon(U markid,const GJK::Mkv* w,Face& f,U e,Face*& cf,Face*& 
448ff)
449                {
450                static const U  mod3[]={0,1,2,0,1};
451                U                               ne(0);
452                if(f.mark!=markid)
453                        {
454                        const U e1(mod3[e+1]);
455                        if((f.n.dot(w->w)+f.d)>0)
456                                {
457                                Face*   nf = NewFace(f.v[e1],f.v[e],w);
458                                Link(nf,0,&f,e);
459                                if(cf) Link(cf,1,nf,2); else ff=nf;
460                                cf=nf;ne=1;
461                                }
462                                else
463                                {
464                                const U e2(mod3[e+2]);
465                                Detach(&f);
466                                f.mark  =       markid;
467                                ne              +=      BuildHorizon(markid,w,*f.f[e1],f.e[e1],cf,ff);
468                                ne              +=      BuildHorizon(markid,w,*f.f[e2],f.e[e2],cf,ff);
469                                }
470                        }
471                return(ne);
472                }
473        //
474        inline F                EvaluatePD(F accuracy=EPA_accuracy)
475                {
476                btBlock*        sablock = sa->beginBlock();
477                Face*                           bestface = 0;
478                U                                       markid(1);
479                depth           =       -cstInf;
480                normal          =       Vector3(0,0,0);
481                root            =       0;
482                nfaces          =       0;
483                iterations      =       0;
484                failed          =       false;
485                /* Prepare hull         */
486                if(gjk->EncloseOrigin())
487                        {
488                        const U*                pfidx = 0;
489                        U                               nfidx= 0;
490                        const U*                peidx = 0;
491                        U                               neidx = 0;
492                        GJK::Mkv*               basemkv[5];
493                        Face*                   basefaces[6];
494                        switch(gjk->order)
495                                {
496                                /* Tetrahedron          */
497                                case    3:      {
498                                                        static const U  fidx[4][3]={{2,1,0},{3,0,1},{3,1,2},{3,2,0}};
499                                                        static const 
500U       eidx[6][4]={{0,0,2,1},{0,1,1,1},{0,2,3,1},{1,0,3,2},{2,0,1,2},{3,0,2,2}};
501                                                        pfidx=(const U*)fidx;nfidx=4;peidx=(const U*)eidx;neidx=6;
502                                                        }       break;
503                                /* Hexahedron           */
504                                case    4:      {
505                                                        static const 
506U       fidx[6][3]={{2,0,4},{4,1,2},{1,4,0},{0,3,1},{0,2,3},{1,3,2}};
507                                                        static const 
508U       eidx[9][4]={{0,0,4,0},{0,1,2,1},{0,2,1,2},{1,1,5,2},{1,0,2,0},{2,2,3,2},{3,1,5,0},{3,0,4,2},{5,1,4,1}};
509                                                        pfidx=(const U*)fidx;nfidx=6;peidx=(const U*)eidx;neidx=9;
510                                                        }       break;
511                                }
512                        U i;
513
514                        for( i=0;i<=gjk->order;++i)             { 
515basemkv[i]=(GJK::Mkv*)sa->allocate(sizeof(GJK::Mkv));*basemkv[i]=gjk->simplex[i]; 
516}
517                        for( i=0;i<nfidx;++i,pfidx+=3)          { 
518basefaces[i]=NewFace(basemkv[pfidx[0]],basemkv[pfidx[1]],basemkv[pfidx[2]]); 
519}
520                        for( i=0;i<neidx;++i,peidx+=4)          { 
521Link(basefaces[peidx[0]],peidx[1],basefaces[peidx[2]],peidx[3]); }
522                        }
523                if(0==nfaces)
524                        {
525                        sa->endBlock(sablock);
526                        return(depth);
527                        }
528                /* Expand hull          */
529                for(;iterations<EPA_maxiterations;++iterations)
530                        {
531                        Face*           bf = FindBest();
532                        if(bf)
533                                {
534                                GJK::Mkv*       w = Support(-bf->n);
535                                const F         d(bf->n.dot(w->w)+bf->d);
536                                bestface        =       bf;
537                                if(d<-accuracy)
538                                        {
539                                        Face*   cf =0;
540                                        Face*   ff =0;
541                                        U               nf = 0;
542                                        Detach(bf);
543                                        bf->mark=++markid;
544                                        for(U i=0;i<3;++i)      { 
545nf+=BuildHorizon(markid,w,*bf->f[i],bf->e[i],cf,ff); }
546                                        if(nf<=2)                       { break; }
547                                        Link(cf,1,ff,2);
548                                        } else break;
549                                } else break;
550                        }
551                /* Extract contact      */
552                if(bestface)
553                        {
554                        const Vector3   b(GetCoordinates(bestface));
555                        normal                  =       bestface->n;
556                        depth                   =       Max<F>(0,bestface->d);
557                        for(U i=0;i<2;++i)
558                                {
559                                const F s(F(i?-1:1));
560                                for(U j=0;j<3;++j)
561                                        {
562                                        features[i][j]=gjk->LocalSupport(s*bestface->v[j]->r,i);
563                                        }
564                                }
565                        nearest[0]              =       features[0][0]*b.x()+features[0][1]*b.y()+features[0][2]*b.z();
566                        nearest[1]              =       features[1][0]*b.x()+features[1][1]*b.y()+features[1][2]*b.z();
567                        } else failed=true;
568                sa->endBlock(sablock);
569                return(depth);
570                }
571        };
572}
573
574//
575// Api
576//
577
578using namespace gjkepa_impl;
579
580
581
582//
583bool    btGjkEpaSolver::Collide(const btConvexShape *shape0,const btTransform &wtrs0,
584                                                                const btConvexShape *shape1,const btTransform &wtrs1,
585                                                                btScalar        radialmargin,
586                                                                btStackAlloc* stackAlloc,
587                                                                sResults&       results)
588{
589       
590
591/* Initialize                                   */
592results.witnesses[0]    =
593results.witnesses[1]    =
594results.normal                  =       Vector3(0,0,0);
595results.depth                   =       0;
596results.status                  =       sResults::Separated;
597results.epa_iterations  =       0;
598results.gjk_iterations  =       0;
599/* Use GJK to locate origin             */
600GJK                     gjk(stackAlloc,
601                                wtrs0.getBasis(),wtrs0.getOrigin(),shape0,
602                                wtrs1.getBasis(),wtrs1.getOrigin(),shape1,
603                                radialmargin+EPA_accuracy);
604const Z         collide(gjk.SearchOrigin());
605results.gjk_iterations  =       static_cast<int>(gjk.iterations+1);
606if(collide)
607        {
608        /* Then EPA for penetration depth       */
609        EPA                     epa(&gjk);
610        const F         pd(epa.EvaluatePD());
611        results.epa_iterations  =       static_cast<int>(epa.iterations+1);
612        if(pd>0)
613                {
614                results.status                  =       sResults::Penetrating;
615                results.normal                  =       epa.normal;
616                results.depth                   =       pd;
617                results.witnesses[0]    =       epa.nearest[0];
618                results.witnesses[1]    =       epa.nearest[1];
619                return(true);
620                } else { if(epa.failed) results.status=sResults::EPA_Failed; }
621        } else { if(gjk.failed) results.status=sResults::GJK_Failed; }
622return(false);
623}
624
625
626
627
628
Note: See TracBrowser for help on using the repository browser.