Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @ 2119

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

Downgraded Bullet to latest tagged version: 2.72
That should give us more stability.
  • Property svn:eol-style set to native
File size: 22.1 KB
Line 
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*/
25#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
26#include "BulletCollision/CollisionShapes/btSphereShape.h"
27#include "btGjkEpa2.h"
28
29#if defined(DEBUG) || defined (_DEBUG)
30#include <stdio.h> //for debug printf
31#ifdef __SPU__
32#include <spu_printf.h>
33#define printf spu_printf
34#endif //__SPU__
35#endif
36
37namespace gjkepa2_impl
38{
39
40// Config
41
42        /* GJK  */ 
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)
50
51        /* EPA  */ 
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)
59
60
61// Shorthands
62typedef unsigned int    U;
63typedef unsigned char   U1;
64
65// MinkowskiDiff
66struct  MinkowskiDiff
67        {
68        const btConvexShape*    m_shapes[2];
69        btMatrix3x3                             m_toshape1;
70        btTransform                             m_toshape0;
71        btVector3                               (btConvexShape::*Ls)(const btVector3&) const;
72        void                                    EnableMargin(bool enable)
73                {
74                if(enable)
75                        Ls=&btConvexShape::localGetSupportVertexNonVirtual;
76                        else
77                        Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual;
78                }       
79        inline btVector3                Support0(const btVector3& d) const
80                {
81                return(((m_shapes[0])->*(Ls))(d));
82                }
83        inline btVector3                Support1(const btVector3& d) const
84                {
85                return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d));
86                }
87        inline btVector3                Support(const btVector3& d) const
88                {
89                return(Support0(d)-Support1(-d));
90                }
91        btVector3                               Support(const btVector3& d,U index) const
92                {
93                if(index)
94                        return(Support1(d));
95                        else
96                        return(Support0(d));
97                }
98        };
99
100typedef MinkowskiDiff   tShape;
101
102
103// GJK
104struct  GJK
105{
106/* Types                */ 
107struct  sSV
108        {
109        btVector3       d,w;
110        };
111struct  sSimplex
112        {
113        sSV*            c[4];
114        btScalar        p[4];
115        U                       rank;
116        };
117struct  eStatus { enum _ {
118        Valid,
119        Inside,
120        Failed          };};
121/* Fields               */ 
122tShape                  m_shape;
123btVector3               m_ray;
124btScalar                m_distance;
125sSimplex                m_simplices[2];
126sSV                             m_store[4];
127sSV*                    m_free[4];
128U                               m_nfree;
129U                               m_current;
130sSimplex*               m_simplex;
131eStatus::_              m_status;
132/* Methods              */ 
133                                        GJK()
134        {
135        Initialize();
136        }
137void                            Initialize()
138        {
139        m_ray           =       btVector3(0,0,0);
140        m_nfree         =       0;
141        m_status        =       eStatus::Failed;
142        m_current       =       0;
143        m_distance      =       0;
144        }
145eStatus::_                      Evaluate(const tShape& shapearg,const btVector3& guess)
146        {
147        U                       iterations=0;
148        btScalar        sqdist=0;
149        btScalar        alpha=0;
150        btVector3       lastw[4];
151        U                       clastw=0;
152        /* Initialize solver            */ 
153        m_free[0]                       =       &m_store[0];
154        m_free[1]                       =       &m_store[1];
155        m_free[2]                       =       &m_store[2];
156        m_free[3]                       =       &m_store[3];
157        m_nfree                         =       4;
158        m_current                       =       0;
159        m_status                        =       eStatus::Valid;
160        m_shape                         =       shapearg;
161        m_distance                      =       0;
162        /* Initialize simplex           */ 
163        m_simplices[0].rank     =       0;
164        m_ray                           =       guess;
165        const btScalar  sqrl=   m_ray.length2();
166        appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0));
167        m_simplices[0].p[0]     =       1;
168        m_ray                           =       m_simplices[0].c[0]->w; 
169        sqdist                          =       sqrl;
170        lastw[0]                        =
171        lastw[1]                        =
172        lastw[2]                        =
173        lastw[3]                        =       m_ray;
174        /* Loop                                         */ 
175        do      {
176                const U         next=1-m_current;
177                sSimplex&       cs=m_simplices[m_current];
178                sSimplex&       ns=m_simplices[next];
179                /* Check zero                                                   */ 
180                const btScalar  rl=m_ray.length();
181                if(rl<GJK_MIN_DISTANCE)
182                        {/* Touching or inside                          */ 
183                        m_status=eStatus::Inside;
184                        break;
185                        }
186                /* Append new vertice in -'v' direction */ 
187                appendvertice(cs,-m_ray);
188                const btVector3&        w=cs.c[cs.rank-1]->w;
189                bool                            found=false;
190                for(U i=0;i<4;++i)
191                        {
192                        if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
193                                { found=true;break; }
194                        }
195                if(found)
196                        {/* Return old simplex                          */ 
197                        removevertice(m_simplices[m_current]);
198                        break;
199                        }
200                        else
201                        {/* Update lastw                                        */ 
202                        lastw[clastw=(clastw+1)&3]=w;
203                        }
204                /* Check for termination                                */ 
205                const btScalar  omega=dot(m_ray,w)/rl;
206                alpha=btMax(omega,alpha);
207                if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
208                        {/* Return old simplex                          */ 
209                        removevertice(m_simplices[m_current]);
210                        break;
211                        }               
212                /* Reduce simplex                                               */ 
213                btScalar        weights[4];
214                U                       mask=0;
215                switch(cs.rank)
216                        {
217                        case    2:      sqdist=projectorigin(   cs.c[0]->w,
218                                                                                                cs.c[1]->w,
219                                                                                                weights,mask);break;
220                        case    3:      sqdist=projectorigin(   cs.c[0]->w,
221                                                                                                cs.c[1]->w,
222                                                                                                cs.c[2]->w,
223                                                                                                weights,mask);break;
224                        case    4:      sqdist=projectorigin(   cs.c[0]->w,
225                                                                                                cs.c[1]->w,
226                                                                                                cs.c[2]->w,
227                                                                                                cs.c[3]->w,
228                                                                                                weights,mask);break;
229                        }
230                if(sqdist>=0)
231                        {/* Valid       */ 
232                        ns.rank         =       0;
233                        m_ray           =       btVector3(0,0,0);
234                        m_current       =       next;
235                        for(U i=0,ni=cs.rank;i<ni;++i)
236                                {
237                                if(mask&(1<<i))
238                                        {
239                                        ns.c[ns.rank]           =       cs.c[i];
240                                        ns.p[ns.rank++]         =       weights[i];
241                                        m_ray                           +=      cs.c[i]->w*weights[i];
242                                        }
243                                        else
244                                        {
245                                        m_free[m_nfree++]       =       cs.c[i];
246                                        }
247                                }
248                        if(mask==15) m_status=eStatus::Inside;
249                        }
250                        else
251                        {/* Return old simplex                          */ 
252                        removevertice(m_simplices[m_current]);
253                        break;
254                        }
255                m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
256                } while(m_status==eStatus::Valid);
257        m_simplex=&m_simplices[m_current];
258        switch(m_status)
259                {
260                case    eStatus::Valid:         m_distance=m_ray.length();break;
261                case    eStatus::Inside:        m_distance=0;break;
262                }       
263        return(m_status);
264        }
265bool                                    EncloseOrigin()
266        {
267        switch(m_simplex->rank)
268                {
269                case    1:
270                        {
271                        for(U i=0;i<3;++i)
272                                {
273                                btVector3               axis=btVector3(0,0,0);
274                                axis[i]=1;
275                                appendvertice(*m_simplex, axis);
276                                if(EncloseOrigin())     return(true);
277                                removevertice(*m_simplex);
278                                appendvertice(*m_simplex,-axis);
279                                if(EncloseOrigin())     return(true);
280                                removevertice(*m_simplex);
281                                }
282                        }
283                break;
284                case    2:
285                        {
286                        const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w;
287                        for(U i=0;i<3;++i)
288                                {
289                                btVector3               axis=btVector3(0,0,0);
290                                axis[i]=1;
291                                const btVector3 p=cross(d,axis);
292                                if(p.length2()>0)
293                                        {
294                                        appendvertice(*m_simplex, p);
295                                        if(EncloseOrigin())     return(true);
296                                        removevertice(*m_simplex);
297                                        appendvertice(*m_simplex,-p);
298                                        if(EncloseOrigin())     return(true);
299                                        removevertice(*m_simplex);
300                                        }
301                                }
302                        }
303                break;
304                case    3:
305                        {
306                        const btVector3 n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w,
307                                                                        m_simplex->c[2]->w-m_simplex->c[0]->w);
308                        if(n.length2()>0)
309                                {
310                                appendvertice(*m_simplex,n);
311                                if(EncloseOrigin())     return(true);
312                                removevertice(*m_simplex);
313                                appendvertice(*m_simplex,-n);
314                                if(EncloseOrigin())     return(true);
315                                removevertice(*m_simplex);
316                                }
317                        }
318                break;
319                case    4:
320                        {
321                        if(btFabs(det(  m_simplex->c[0]->w-m_simplex->c[3]->w,
322                                                        m_simplex->c[1]->w-m_simplex->c[3]->w,
323                                                        m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
324                                return(true);
325                        }
326                break;
327                }
328        return(false);
329        }
330/* Internals    */ 
331void                            getsupport(const btVector3& d,sSV& sv) const
332        {
333        sv.d    =       d/d.length();
334        sv.w    =       m_shape.Support(sv.d);
335        }
336void                            removevertice(sSimplex& simplex)
337        {
338        m_free[m_nfree++]=simplex.c[--simplex.rank];
339        }
340void                            appendvertice(sSimplex& simplex,const btVector3& v)
341        {
342        simplex.p[simplex.rank]=0;
343        simplex.c[simplex.rank]=m_free[--m_nfree];
344        getsupport(v,*simplex.c[simplex.rank++]);
345        }
346static btScalar         det(const btVector3& a,const btVector3& b,const btVector3& c)
347        {
348        return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()-
349                        a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+
350                        a.x()*b.y()*c.z()-a.z()*b.y()*c.x());
351        }
352static btScalar         projectorigin(  const btVector3& a,
353                                                                        const btVector3& b,
354                                                                        btScalar* w,U& m)
355        {
356        const btVector3 d=b-a;
357        const btScalar  l=d.length2();
358        if(l>GJK_SIMPLEX2_EPS)
359                {
360                const btScalar  t(l>0?-dot(a,d)/l:0);
361                if(t>=1)                { w[0]=0;w[1]=1;m=2;return(b.length2()); }
362                else if(t<=0)   { w[0]=1;w[1]=0;m=1;return(a.length2()); }
363                else                    { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
364                }
365        return(-1);
366        }
367static btScalar         projectorigin(  const btVector3& a,
368                                                                        const btVector3& b,
369                                                                        const btVector3& c,
370                                                                        btScalar* w,U& m)
371        {
372        static const U          imd3[]={1,2,0};
373        const btVector3*        vt[]={&a,&b,&c};
374        const btVector3         dl[]={a-b,b-c,c-a};
375        const btVector3         n=cross(dl[0],dl[1]);
376        const btScalar          l=n.length2();
377        if(l>GJK_SIMPLEX3_EPS)
378                {
379                btScalar        mindist=-1;
380                btScalar        subw[2];
381                U                       subm;
382                for(U i=0;i<3;++i)
383                        {
384                        if(dot(*vt[i],cross(dl[i],n))>0)
385                                {
386                                const U                 j=imd3[i];
387                                const btScalar  subd(projectorigin(*vt[i],*vt[j],subw,subm));
388                                if((mindist<0)||(subd<mindist))
389                                        {
390                                        mindist         =       subd;
391                                        m                       =       static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
392                                        w[i]            =       subw[0];
393                                        w[j]            =       subw[1];
394                                        w[imd3[j]]      =       0;                             
395                                        }
396                                }
397                        }
398                if(mindist<0)
399                        {
400                        const btScalar  d=dot(a,n);     
401                        const btScalar  s=btSqrt(l);
402                        const btVector3 p=n*(d/l);
403                        mindist =       p.length2();
404                        m               =       7;
405                        w[0]    =       (cross(dl[1],b-p)).length()/s;
406                        w[1]    =       (cross(dl[2],c-p)).length()/s;
407                        w[2]    =       1-(w[0]+w[1]);
408                        }
409                return(mindist);
410                }
411        return(-1);
412        }
413static btScalar         projectorigin(  const btVector3& a,
414                                                                        const btVector3& b,
415                                                                        const btVector3& c,
416                                                                        const btVector3& d,
417                                                                        btScalar* w,U& m)
418        {
419        static const U          imd3[]={1,2,0};
420        const btVector3*        vt[]={&a,&b,&c,&d};
421        const btVector3         dl[]={a-d,b-d,c-d};
422        const btScalar          vl=det(dl[0],dl[1],dl[2]);
423        const bool                      ng=(vl*dot(a,cross(b-c,a-b)))<=0;
424        if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
425                {
426                btScalar        mindist=-1;
427                btScalar        subw[3];
428                U                       subm;
429                for(U i=0;i<3;++i)
430                        {
431                        const U                 j=imd3[i];
432                        const btScalar  s=vl*dot(d,cross(dl[i],dl[j]));
433                        if(s>0)
434                                {
435                                const btScalar  subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
436                                if((mindist<0)||(subd<mindist))
437                                        {
438                                        mindist         =       subd;
439                                        m                       =       static_cast<U>((subm&1?1<<i:0)+
440                                                                        (subm&2?1<<j:0)+
441                                                                        (subm&4?8:0));
442                                        w[i]            =       subw[0];
443                                        w[j]            =       subw[1];
444                                        w[imd3[j]]      =       0;
445                                        w[3]            =       subw[2];
446                                        }
447                                }
448                        }
449                if(mindist<0)
450                        {
451                        mindist =       0;
452                        m               =       15;
453                        w[0]    =       det(c,b,d)/vl;
454                        w[1]    =       det(a,c,d)/vl;
455                        w[2]    =       det(b,a,d)/vl;
456                        w[3]    =       1-(w[0]+w[1]+w[2]);
457                        }
458                return(mindist);
459                }
460        return(-1);
461        }
462};
463
464// EPA
465struct  EPA
466{
467/* Types                */ 
468typedef GJK::sSV        sSV;
469struct  sFace
470        {
471        btVector3       n;
472        btScalar        d;
473        btScalar        p;
474        sSV*            c[3];
475        sFace*          f[3];
476        sFace*          l[2];
477        U1                      e[3];
478        U1                      pass;
479        };
480struct  sList
481        {
482        sFace*          root;
483        U                       count;
484                                sList() : root(0),count(0)      {}
485        };
486struct  sHorizon
487        {
488        sFace*          cf;
489        sFace*          ff;
490        U                       nf;
491                                sHorizon() : cf(0),ff(0),nf(0)  {}
492        };
493struct  eStatus { enum _ {
494        Valid,
495        Touching,
496        Degenerated,
497        NonConvex,
498        InvalidHull,           
499        OutOfFaces,
500        OutOfVertices,
501        AccuraryReached,
502        FallBack,
503        Failed          };};
504/* Fields               */ 
505eStatus::_              m_status;
506GJK::sSimplex   m_result;
507btVector3               m_normal;
508btScalar                m_depth;
509sSV                             m_sv_store[EPA_MAX_VERTICES];
510sFace                   m_fc_store[EPA_MAX_FACES];
511U                               m_nextsv;
512sList                   m_hull;
513sList                   m_stock;
514/* Methods              */ 
515                                        EPA()
516        {
517        Initialize();   
518        }
519
520
521                                        static inline void              bind(sFace* fa,U ea,sFace* fb,U eb)
522        {
523        fa->e[ea]=(U1)eb;fa->f[ea]=fb;
524        fb->e[eb]=(U1)ea;fb->f[eb]=fa;
525        }
526static inline void              append(sList& list,sFace* face)
527        {
528        face->l[0]      =       0;
529        face->l[1]      =       list.root;
530        if(list.root) list.root->l[0]=face;
531        list.root       =       face;
532        ++list.count;
533        }
534static inline void              remove(sList& list,sFace* face)
535        {
536        if(face->l[1]) face->l[1]->l[0]=face->l[0];
537        if(face->l[0]) face->l[0]->l[1]=face->l[1];
538        if(face==list.root) list.root=face->l[1];
539        --list.count;
540        }
541
542
543void                            Initialize()
544        {
545        m_status        =       eStatus::Failed;
546        m_normal        =       btVector3(0,0,0);
547        m_depth         =       0;
548        m_nextsv        =       0;
549        for(U i=0;i<EPA_MAX_FACES;++i)
550                {
551                append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
552                }
553        }
554eStatus::_                      Evaluate(GJK& gjk,const btVector3& guess)
555        {
556        GJK::sSimplex&  simplex=*gjk.m_simplex;
557        if((simplex.rank>1)&&gjk.EncloseOrigin())
558                {
559
560                /* Clean up                             */ 
561                while(m_hull.root)
562                        {
563                        sFace*  f = m_hull.root;
564                        remove(m_hull,f);
565                        append(m_stock,f);
566                        }
567                m_status        =       eStatus::Valid;
568                m_nextsv        =       0;
569                /* Orient simplex               */ 
570                if(gjk.det(     simplex.c[0]->w-simplex.c[3]->w,
571                                        simplex.c[1]->w-simplex.c[3]->w,
572                                        simplex.c[2]->w-simplex.c[3]->w)<0)
573                        {
574                        btSwap(simplex.c[0],simplex.c[1]);
575                        btSwap(simplex.p[0],simplex.p[1]);
576                        }
577                /* Build initial hull   */ 
578                sFace*  tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
579                                                newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
580                                                newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
581                                                newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
582                if(m_hull.count==4)
583                        {
584                        sFace*          best=findbest();
585                        sFace           outer=*best;
586                        U                       pass=0;
587                        U                       iterations=0;
588                        bind(tetra[0],0,tetra[1],0);
589                        bind(tetra[0],1,tetra[2],0);
590                        bind(tetra[0],2,tetra[3],0);
591                        bind(tetra[1],1,tetra[3],2);
592                        bind(tetra[1],2,tetra[2],1);
593                        bind(tetra[2],2,tetra[3],1);
594                        m_status=eStatus::Valid;
595                        for(;iterations<EPA_MAX_ITERATIONS;++iterations)
596                                {
597                                if(m_nextsv<EPA_MAX_VERTICES)
598                                        {       
599                                        sHorizon                horizon;
600                                        sSV*                    w=&m_sv_store[m_nextsv++];
601                                        bool                    valid=true;                                     
602                                        best->pass      =       (U1)(++pass);
603                                        gjk.getsupport(best->n,*w);
604                                        const btScalar  wdist=dot(best->n,w->w)-best->d;
605                                        if(wdist>EPA_ACCURACY)
606                                                {
607                                                for(U j=0;(j<3)&&valid;++j)
608                                                        {
609                                                        valid&=expand(  pass,w,
610                                                                                        best->f[j],best->e[j],
611                                                                                        horizon);
612                                                        }
613                                                if(valid&&(horizon.nf>=3))
614                                                        {
615                                                        bind(horizon.cf,1,horizon.ff,2);
616                                                        remove(m_hull,best);
617                                                        append(m_stock,best);
618                                                        best=findbest();
619                                                        if(best->p>=outer.p) outer=*best;
620                                                        } else { m_status=eStatus::InvalidHull;break; }
621                                                } else { m_status=eStatus::AccuraryReached;break; }
622                                        } else { m_status=eStatus::OutOfVertices;break; }
623                                }
624                        const btVector3 projection=outer.n*outer.d;
625                        m_normal        =       outer.n;
626                        m_depth         =       outer.d;
627                        m_result.rank   =       3;
628                        m_result.c[0]   =       outer.c[0];
629                        m_result.c[1]   =       outer.c[1];
630                        m_result.c[2]   =       outer.c[2];
631                        m_result.p[0]   =       cross(  outer.c[1]->w-projection,
632                                                                                outer.c[2]->w-projection).length();
633                        m_result.p[1]   =       cross(  outer.c[2]->w-projection,
634                                                                                outer.c[0]->w-projection).length();
635                        m_result.p[2]   =       cross(  outer.c[0]->w-projection,
636                                                                                outer.c[1]->w-projection).length();
637                        const btScalar  sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
638                        m_result.p[0]   /=      sum;
639                        m_result.p[1]   /=      sum;
640                        m_result.p[2]   /=      sum;
641                        return(m_status);
642                        }
643                }
644        /* Fallback             */ 
645        m_status        =       eStatus::FallBack;
646        m_normal        =       -guess;
647        const btScalar  nl=m_normal.length();
648        if(nl>0)
649                m_normal        =       m_normal/nl;
650                else
651                m_normal        =       btVector3(1,0,0);
652        m_depth =       0;
653        m_result.rank=1;
654        m_result.c[0]=simplex.c[0];
655        m_result.p[0]=1;       
656        return(m_status);
657        }
658sFace*                          newface(sSV* a,sSV* b,sSV* c,bool forced)
659        {
660        if(m_stock.root)
661                {
662                sFace*  face=m_stock.root;
663                remove(m_stock,face);
664                append(m_hull,face);
665                face->pass      =       0;
666                face->c[0]      =       a;
667                face->c[1]      =       b;
668                face->c[2]      =       c;
669                face->n         =       cross(b->w-a->w,c->w-a->w);
670                const btScalar  l=face->n.length();
671                const bool              v=l>EPA_ACCURACY;
672                face->p         =       btMin(btMin(
673                                                        dot(a->w,cross(face->n,a->w-b->w)),
674                                                        dot(b->w,cross(face->n,b->w-c->w))),
675                                                        dot(c->w,cross(face->n,c->w-a->w)))     /
676                                                        (v?l:1);
677                face->p         =       face->p>=-EPA_INSIDE_EPS?0:face->p;
678                if(v)
679                        {
680                        face->d         =       dot(a->w,face->n)/l;
681                        face->n         /=      l;
682                        if(forced||(face->d>=-EPA_PLANE_EPS))
683                                {
684                                return(face);
685                                } else m_status=eStatus::NonConvex;
686                        } else m_status=eStatus::Degenerated;
687                remove(m_hull,face);
688                append(m_stock,face);
689                return(0);
690                }
691        m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces;
692        return(0);
693        }
694sFace*                          findbest()
695        {
696        sFace*          minf=m_hull.root;
697        btScalar        mind=minf->d*minf->d;
698        btScalar        maxp=minf->p;
699        for(sFace* f=minf->l[1];f;f=f->l[1])
700                {
701                const btScalar  sqd=f->d*f->d;
702                if((f->p>=maxp)&&(sqd<mind))
703                        {
704                        minf=f;
705                        mind=sqd;
706                        maxp=f->p;
707                        }
708                }
709        return(minf);
710        }
711bool                            expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
712        {
713        static const U  i1m3[]={1,2,0};
714        static const U  i2m3[]={2,0,1};
715        if(f->pass!=pass)
716                {
717                const U e1=i1m3[e];
718                if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
719                        {
720                        sFace*  nf=newface(f->c[e1],f->c[e],w,false);
721                        if(nf)
722                                {
723                                bind(nf,0,f,e);
724                                if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
725                                horizon.cf=nf;
726                                ++horizon.nf;
727                                return(true);
728                                }
729                        }
730                        else
731                        {
732                        const U e2=i2m3[e];
733                        f->pass         =       (U1)pass;
734                        if(     expand(pass,w,f->f[e1],f->e[e1],horizon)&&
735                                expand(pass,w,f->f[e2],f->e[e2],horizon))
736                                {
737                                remove(m_hull,f);
738                                append(m_stock,f);
739                                return(true);
740                                }
741                        }
742                }
743        return(false);
744        }
745
746};
747
748//
749static void     Initialize(     const btConvexShape* shape0,const btTransform& wtrs0,
750                                                const btConvexShape* shape1,const btTransform& wtrs1,
751                                                btGjkEpaSolver2::sResults& results,
752                                                tShape& shape,
753                                                bool withmargins)
754{
755/* Results              */ 
756results.witnesses[0]    =
757results.witnesses[1]    =       btVector3(0,0,0);
758results.status                  =       btGjkEpaSolver2::sResults::Separated;
759/* Shape                */ 
760shape.m_shapes[0]               =       shape0;
761shape.m_shapes[1]               =       shape1;
762shape.m_toshape1                =       wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
763shape.m_toshape0                =       wtrs0.inverseTimes(wtrs1);
764shape.EnableMargin(withmargins);
765}
766
767}
768
769//
770// Api
771//
772
773using namespace gjkepa2_impl;
774
775//
776int                     btGjkEpaSolver2::StackSizeRequirement()
777{
778return(sizeof(GJK)+sizeof(EPA));
779}
780
781//
782bool            btGjkEpaSolver2::Distance(      const btConvexShape*    shape0,
783                                                                                const btTransform&              wtrs0,
784                                                                                const btConvexShape*    shape1,
785                                                                                const btTransform&              wtrs1,
786                                                                                const btVector3&                guess,
787                                                                                sResults&                               results)
788{
789tShape                  shape;
790Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
791GJK                             gjk;
792GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
793if(gjk_status==GJK::eStatus::Valid)
794        {
795        btVector3       w0=btVector3(0,0,0);
796        btVector3       w1=btVector3(0,0,0);
797        for(U i=0;i<gjk.m_simplex->rank;++i)
798                {
799                const btScalar  p=gjk.m_simplex->p[i];
800                w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
801                w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
802                }
803        results.witnesses[0]    =       wtrs0*w0;
804        results.witnesses[1]    =       wtrs0*w1;
805        results.normal                  =       w0-w1;
806        results.distance                =       results.normal.length();
807        results.normal                  /=      results.distance>GJK_MIN_DISTANCE?results.distance:1;
808        return(true);
809        }
810        else
811        {
812        results.status  =       gjk_status==GJK::eStatus::Inside?
813                                                        sResults::Penetrating   :
814                                                        sResults::GJK_Failed    ;
815        return(false);
816        }
817}
818
819//
820bool    btGjkEpaSolver2::Penetration(   const btConvexShape*    shape0,
821                                                                                const btTransform&              wtrs0,
822                                                                                const btConvexShape*    shape1,
823                                                                                const btTransform&              wtrs1,
824                                                                                const btVector3&                guess,
825                                                                                sResults&                               results,
826                                                                                bool                                    usemargins)
827{
828tShape                  shape;
829Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
830GJK                             gjk;   
831GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess);
832switch(gjk_status)
833        {
834        case    GJK::eStatus::Inside:
835                {
836                EPA                             epa;
837                EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess);
838                if(epa_status!=EPA::eStatus::Failed)
839                        {
840                        btVector3       w0=btVector3(0,0,0);
841                        for(U i=0;i<epa.m_result.rank;++i)
842                                {
843                                w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
844                                }
845                        results.status                  =       sResults::Penetrating;
846                        results.witnesses[0]    =       wtrs0*w0;
847                        results.witnesses[1]    =       wtrs0*(w0-epa.m_normal*epa.m_depth);
848                        results.normal                  =       -epa.m_normal;
849                        results.distance                =       -epa.m_depth;
850                        return(true);
851                        } else results.status=sResults::EPA_Failed;
852                }
853        break;
854        case    GJK::eStatus::Failed:
855        results.status=sResults::GJK_Failed;
856        break;
857        }
858return(false);
859}
860
861//
862btScalar        btGjkEpaSolver2::SignedDistance(const btVector3& position,
863                                                                                        btScalar margin,
864                                                                                        const btConvexShape* shape0,
865                                                                                        const btTransform& wtrs0,
866                                                                                        sResults& results)
867{
868tShape                  shape;
869btSphereShape   shape1(margin);
870btTransform             wtrs1(btQuaternion(0,0,0,1),position);
871Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
872GJK                             gjk;   
873GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));
874if(gjk_status==GJK::eStatus::Valid)
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]    =       wtrs0*w0;
885        results.witnesses[1]    =       wtrs0*w1;
886        const btVector3 delta=  results.witnesses[1]-
887                                                        results.witnesses[0];
888        const btScalar  margin= shape0->getMargin()+
889                                                        shape1.getMargin();
890        const btScalar  length= delta.length(); 
891        results.normal                  =       delta/length;
892        results.witnesses[0]    +=      results.normal*margin;
893        return(length-margin);
894        }
895        else
896        {
897        if(gjk_status==GJK::eStatus::Inside)
898                {
899                if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results))
900                        {
901                        const btVector3 delta=  results.witnesses[0]-
902                                                                        results.witnesses[1];
903                        const btScalar  length= delta.length();
904                        if (length >= SIMD_EPSILON)
905                                results.normal  =       delta/length;                   
906                        return(-length);
907                        }
908                }       
909        }
910return(SIMD_INFINITY);
911}
912
913//
914bool    btGjkEpaSolver2::SignedDistance(const btConvexShape*    shape0,
915                                                                                const btTransform&              wtrs0,
916                                                                                const btConvexShape*    shape1,
917                                                                                const btTransform&              wtrs1,
918                                                                                const btVector3&                guess,
919                                                                                sResults&                               results)
920{
921if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
922        return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false));
923        else
924        return(true);
925}
926
927/* Symbols cleanup              */ 
928
929#undef GJK_MAX_ITERATIONS
930#undef GJK_ACCURARY
931#undef GJK_MIN_DISTANCE
932#undef GJK_DUPLICATED_EPS
933#undef GJK_SIMPLEX2_EPS
934#undef GJK_SIMPLEX3_EPS
935#undef GJK_SIMPLEX4_EPS
936
937#undef EPA_MAX_VERTICES
938#undef EPA_MAX_FACES
939#undef EPA_MAX_ITERATIONS
940#undef EPA_ACCURACY
941#undef EPA_FALLBACK
942#undef EPA_PLANE_EPS
943#undef EPA_INSIDE_EPS
Note: See TracBrowser for help on using the repository browser.