Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/pickups/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @ 2921

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

Merged presentation branch back to trunk.

  • Property svn:eol-style set to native
File size: 23.6 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
62        typedef unsigned int    U;
63        typedef unsigned char   U1;
64
65        // MinkowskiDiff
66        struct  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
100        typedef MinkowskiDiff   tShape;
101
102
103        // GJK
104        struct  GJK
105        {
106                /* Types                */ 
107                struct  sSV
108                {
109                        btVector3       d,w;
110                };
111                struct  sSimplex
112                {
113                        sSV*            c[4];
114                        btScalar        p[4];
115                        U                       rank;
116                };
117                struct  eStatus { enum _ {
118                        Valid,
119                        Inside,
120                        Failed          };};
121                        /* Fields               */ 
122                        tShape                  m_shape;
123                        btVector3               m_ray;
124                        btScalar                m_distance;
125                        sSimplex                m_simplices[2];
126                        sSV                             m_store[4];
127                        sSV*                    m_free[4];
128                        U                               m_nfree;
129                        U                               m_current;
130                        sSimplex*               m_simplex;
131                        eStatus::_              m_status;
132                        /* Methods              */ 
133                        GJK()
134                        {
135                                Initialize();
136                        }
137                        void                            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                        }
145                        eStatus::_                      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                        }
265                        bool                                    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    */ 
331                        void                            getsupport(const btVector3& d,sSV& sv) const
332                        {
333                                sv.d    =       d/d.length();
334                                sv.w    =       m_shape.Support(sv.d);
335                        }
336                        void                            removevertice(sSimplex& simplex)
337                        {
338                                m_free[m_nfree++]=simplex.c[--simplex.rank];
339                        }
340                        void                            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                        }
346                        static 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                        }
352                        static 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                        }
367                        static 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                        }
413                        static 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
465        struct  EPA
466        {
467                /* Types                */ 
468                typedef GJK::sSV        sSV;
469                struct  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                };
480                struct  sList
481                {
482                        sFace*          root;
483                        U                       count;
484                        sList() : root(0),count(0)      {}
485                };
486                struct  sHorizon
487                {
488                        sFace*          cf;
489                        sFace*          ff;
490                        U                       nf;
491                        sHorizon() : cf(0),ff(0),nf(0)  {}
492                };
493                struct  eStatus { enum _ {
494                        Valid,
495                        Touching,
496                        Degenerated,
497                        NonConvex,
498                        InvalidHull,           
499                        OutOfFaces,
500                        OutOfVertices,
501                        AccuraryReached,
502                        FallBack,
503                        Failed          };};
504                        /* Fields               */ 
505                        eStatus::_              m_status;
506                        GJK::sSimplex   m_result;
507                        btVector3               m_normal;
508                        btScalar                m_depth;
509                        sSV                             m_sv_store[EPA_MAX_VERTICES];
510                        sFace                   m_fc_store[EPA_MAX_FACES];
511                        U                               m_nextsv;
512                        sList                   m_hull;
513                        sList                   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                        }
526                        static 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                        }
534                        static 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
543                        void                            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                        }
554                        eStatus::_                      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                        }
658                        sFace*                          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                        }
694                        sFace*                          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                        }
711                        bool                            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        //
749        static 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              */ 
756                results.witnesses[0]    =
757                        results.witnesses[1]    =       btVector3(0,0,0);
758                results.status                  =       btGjkEpaSolver2::sResults::Separated;
759                /* Shape                */ 
760                shape.m_shapes[0]               =       shape0;
761                shape.m_shapes[1]               =       shape1;
762                shape.m_toshape1                =       wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
763                shape.m_toshape0                =       wtrs0.inverseTimes(wtrs1);
764                shape.EnableMargin(withmargins);
765        }
766
767}
768
769//
770// Api
771//
772
773using namespace gjkepa2_impl;
774
775//
776int                     btGjkEpaSolver2::StackSizeRequirement()
777{
778        return(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{
789        tShape                  shape;
790        Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
791        GJK                             gjk;
792        GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
793        if(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{
828        tShape                  shape;
829        Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
830        GJK                             gjk;   
831        GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess);
832        switch(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        }
858        return(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{
868        tShape                  shape;
869        btSphereShape   shape1(margin);
870        btTransform             wtrs1(btQuaternion(0,0,0,1),position);
871        Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
872        GJK                             gjk;   
873        GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));
874        if(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->getMarginNonVirtual()+
889                        shape1.getMarginNonVirtual();
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        }
910        return(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{
921        if(!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.