← Back to branch summary

~ubuntu-branches/ubuntu/intrepid/blender/intrepid-updates

« back to all changes in this revision

Viewing changes to extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp

  • Committer: Bazaar Package Importer
  • Author(s): Cyril Brulebois
  • Date: 2008-08-08 02:45:40 UTC
  • mfrom: (12.1.14 intrepid)
  • Revision ID: james.westby@ubuntu.com-20080808024540-kkjp7ekfivzhuw3l
Tags: 2.46+dfsg-4
http://bugs.debian.org/492280
* Fix python syntax warning in import_dxf.py, which led to nasty output
  in installation/upgrade logs during byte-compilation, using a patch
  provided by the script author (Closes: #492280):
   - debian/patches/45_fix_python_syntax_warning

Show diffs side-by-side

added added

removed removed

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