Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/hudHS14/src/external/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @ 10222

Last change on this file since 10222 was 8351, checked in by rgrieder, 14 years ago

Merged kicklib2 branch back to trunk (includes former branches ois_update, mac_osx and kicklib).

Notes for updating

Linux:
You don't need an extra package for CEGUILua and Tolua, it's already shipped with CEGUI.
However you do need to make sure that the OgreRenderer is installed too with CEGUI 0.7 (may be a separate package).
Also, Orxonox now recognises if you install the CgProgramManager (a separate package available on newer Ubuntu on Debian systems).

Windows:
Download the new dependency packages versioned 6.0 and use these. If you have problems with that or if you don't like the in game console problem mentioned below, you can download the new 4.3 version of the packages (only available for Visual Studio 2005/2008).

Key new features:

  • *Support for Mac OS X*
  • Visual Studio 2010 support
  • Bullet library update to 2.77
  • OIS library update to 1.3
  • Support for CEGUI 0.7 —> Support for Arch Linux and even SuSE
  • Improved install target
  • Compiles now with GCC 4.6
  • Ogre Cg Shader plugin activated for Linux if available
  • And of course lots of bug fixes

There are also some regressions:

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